diff --git a/.github/workflows/conda/environment_macos_linux.yml b/.github/workflows/conda/environment_macos_linux.yml index 18191fd5a..34a295bad 100644 --- a/.github/workflows/conda/environment_macos_linux.yml +++ b/.github/workflows/conda/environment_macos_linux.yml @@ -1,4 +1,4 @@ -name: fcl +name: coal channels: - conda-forge - nodefaults @@ -10,7 +10,7 @@ dependencies: - boost - eigenpy - python - - doxygen + - doxygen<1.9.8|>=1.11 - lxml - pylatexenc - qhull diff --git a/.github/workflows/conda/environment_windows.yml b/.github/workflows/conda/environment_windows.yml index 8c6fccaed..febe634ee 100644 --- a/.github/workflows/conda/environment_windows.yml +++ b/.github/workflows/conda/environment_windows.yml @@ -1,4 +1,4 @@ -name: fcl +name: coal channels: - conda-forge - nodefaults @@ -10,7 +10,7 @@ dependencies: - boost - eigenpy - python - - doxygen + - doxygen<1.9.8|>=1.11 - lxml - pylatexenc - qhull diff --git a/.github/workflows/macos-linux-conda.yml b/.github/workflows/macos-linux-conda.yml index a875f175c..913ed513b 100644 --- a/.github/workflows/macos-linux-conda.yml +++ b/.github/workflows/macos-linux-conda.yml @@ -1,9 +1,9 @@ -name: Build hpp-fcl for Mac OS X/Linux via Conda +name: Build Coal for Mac OS X/Linux via Conda on: [push,pull_request] jobs: - hpp-fcl-conda: + coal-conda: name: CI on ${{ matrix.os }} with Conda Python ${{ matrix.python-version }} - ${{ matrix.build_type }} ${{ matrix.cxx_options }} runs-on: ${{ matrix.os }} env: @@ -40,13 +40,13 @@ jobs: - uses: conda-incubator/setup-miniconda@v3 with: - activate-environment: fcl + activate-environment: coal auto-update-conda: true environment-file: .github/workflows/conda/environment_macos_linux.yml python-version: ${{ matrix.python-version }} auto-activate-base: false - - name: Build hpp-fcl + - name: Build Coal shell: bash -el {0} run: | conda list @@ -65,14 +65,14 @@ jobs: -DCMAKE_CXX_FLAGS=${{ matrix.cxx_options }} \ -DPYTHON_EXECUTABLE=$(which python3) \ -DGENERATE_PYTHON_STUBS=ON \ - -DHPP_FCL_HAS_QHULL=ON \ + -DCOAL_HAS_QHULL=ON \ -DBUILD_DOCUMENTATION=ON \ -DINSTALL_DOCUMENTATION=ON cmake --build . -j2 ctest --output-on-failure cmake --install . - - name: Uninstall hpp-fcl + - name: Uninstall Coal shell: bash -el {0} run: | cd build @@ -83,7 +83,7 @@ jobs: name: check-macos-linux-conda needs: - - hpp-fcl-conda + - coal-conda runs-on: Ubuntu-latest diff --git a/.github/workflows/macos-linux-pip.yml b/.github/workflows/macos-linux-pip.yml index 2ced0264f..156a86c1e 100644 --- a/.github/workflows/macos-linux-pip.yml +++ b/.github/workflows/macos-linux-pip.yml @@ -1,4 +1,4 @@ -name: Build hpp-fcl for Mac OS X/Linux via pip +name: Build Coal for Mac OS X/Linux via pip on: [push, pull_request] @@ -7,7 +7,7 @@ env: CTEST_PARALLEL_LEVEL: 4 jobs: - hpp-fcl-pip: + coal-pip: name: "CI on ${{ matrix.os }} / py ${{ matrix.python-version }} with pip" runs-on: "${{ matrix.os }}-latest" @@ -29,6 +29,6 @@ jobs: - run: python -m pip install -U pip - run: python -m pip install cmeel-assimp cmeel-octomap cmeel-qhull eigenpy[build] - run: echo "CMAKE_PREFIX_PATH=$(cmeel cmake)" >> $GITHUB_ENV - - run: cmake -B build -S . -DHPP_FCL_HAS_QHULL=ON + - run: cmake -B build -S . -DCOAL_HAS_QHULL=ON - run: cmake --build build -j 4 - run: cmake --build build -t test diff --git a/.github/workflows/windows-conda-clang.yml b/.github/workflows/windows-conda-clang.yml index 33fc913ca..7d7a8d87a 100644 --- a/.github/workflows/windows-conda-clang.yml +++ b/.github/workflows/windows-conda-clang.yml @@ -1,4 +1,4 @@ -name: Build FCL for Windows (CLANG) via Conda +name: Build Coal for Windows (CLANG) via Conda on: [push,pull_request] jobs: @@ -33,13 +33,13 @@ jobs: - uses: conda-incubator/setup-miniconda@v3 with: - activate-environment: fcl + activate-environment: coal auto-update-conda: true environment-file: .github/workflows/conda/environment_windows.yml python-version: "3.10" auto-activate-base: false - - name: Build FCL + - name: Build Coal shell: cmd /C CALL {0} run: | call "%programfiles(x86)%\Microsoft Visual Studio\2019\Enterprise\VC\Auxiliary\Build\vcvarsall.bat" amd64 @@ -59,7 +59,7 @@ jobs: -DGENERATE_PYTHON_STUBS=ON ^ -DPYTHON_SITELIB=%CONDA_PREFIX%\Lib\site-packages ^ -DPYTHON_EXECUTABLE=%CONDA_PREFIX%\python.exe ^ - -DHPP_FCL_HAS_QHULL=ON ^ + -DCOAL_HAS_QHULL=ON ^ -DBUILD_PYTHON_INTERFACE=ON ^ .. if errorlevel 1 exit 1 @@ -74,7 +74,7 @@ jobs: :: Test Python import cd .. - python -c "import hppfcl" + python -c "import coal" if errorlevel 1 exit 1 check: diff --git a/.github/workflows/windows-conda-v142.yml b/.github/workflows/windows-conda-v142.yml index e09444ba3..e1191d014 100644 --- a/.github/workflows/windows-conda-v142.yml +++ b/.github/workflows/windows-conda-v142.yml @@ -1,4 +1,4 @@ -name: Build FCL for Windows (v142) via Conda +name: Build Coal for Windows (v142) via Conda on: [push,pull_request] jobs: @@ -32,13 +32,13 @@ jobs: - uses: conda-incubator/setup-miniconda@v3 with: - activate-environment: fcl + activate-environment: coal auto-update-conda: true environment-file: .github/workflows/conda/environment_windows.yml python-version: "3.10" auto-activate-base: false - - name: Build FCL + - name: Build Coal shell: cmd /C CALL {0} run: | call "%programfiles(x86)%\Microsoft Visual Studio\2019\Enterprise\VC\Auxiliary\Build\vcvarsall.bat" amd64 @@ -56,7 +56,7 @@ jobs: -DGENERATE_PYTHON_STUBS=ON ^ -DPYTHON_SITELIB=%CONDA_PREFIX%\Lib\site-packages ^ -DPYTHON_EXECUTABLE=%CONDA_PREFIX%\python.exe ^ - -DHPP_FCL_HAS_QHULL=ON ^ + -DCOAL_HAS_QHULL=ON ^ -DBUILD_PYTHON_INTERFACE=ON ^ .. if errorlevel 1 exit 1 @@ -71,7 +71,7 @@ jobs: :: Test Python import cd .. - python -c "import hppfcl" + python -c "import coal" if errorlevel 1 exit 1 check: diff --git a/CHANGELOG.md b/CHANGELOG.md index 1f3603801..990ca9d7b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ## [Unreleased] ### Added +- Renaming the library from `hpp-fcl` to `coal`. Created a `COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL` CMake option for retro compatibility. This allows to still do `find_package(hpp-fcl)` and `#include ` in C++ and it allows to still do `import hppfcl` in python ([#596](https://github.com/humanoid-path-planner/hpp-fcl/pull/596)). - Added `Transform3f::Random` and `Transform3f::setRandom` ([#584](https://github.com/humanoid-path-planner/hpp-fcl/pull/584)) - New feature: computation of contact surfaces for any pair of primitive shapes (triangle, sphere, ellipsoid, plane, halfspace, cone, capsule, cylinder, convex) ([#574](https://github.com/humanoid-path-planner/hpp-fcl/pull/574)). - Enhance Broadphase DynamicAABBTree to better handle planes and halfspace ([#570](https://github.com/humanoid-path-planner/hpp-fcl/pull/570)) @@ -22,8 +23,10 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Optimize EPA: ignore useless faces in EPA's polytope; warm-start support computation for `Convex`; fix edge-cases witness points computation. - Add `Serializable` trait to transform, collision data, collision geometries, bounding volumes, bvh models, hfields. Collision problems can now be serialized from C++ and sent to python and vice versa. - CMake: allow use of installed jrl-cmakemodules ([#564](https://github.com/humanoid-path-planner/hpp-fcl/pull/564)) +- CMake: Add compatibility with jrl-cmakemodules workspace ([#610](https://github.com/humanoid-path-planner/hpp-fcl/pull/610)) - Python: add id() support for geometries ([#618](https://github.com/humanoid-path-planner/hpp-fcl/pull/618)). + ### Fixed - Fix Fix serialization unit test when running without Qhull support ([#611](https://github.com/humanoid-path-planner/hpp-fcl/pull/611)) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7d5031b66..72f69ead4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,12 +35,16 @@ cmake_minimum_required(VERSION 3.10) set(CXX_DISABLE_WERROR TRUE) -set(PROJECT_NAME hpp-fcl) +set(PROJECT_NAME coal) set(PROJECT_DESCRIPTION - "HPP fork of FCL -- The Flexible Collision Library" + "Coal, The Collision Detection Library. Previously known as HPP-FCL, fork of FCL -- The Flexible Collision Library" ) SET(PROJECT_USE_CMAKE_EXPORT TRUE) SET(PROJECT_COMPATIBILITY_VERSION AnyNewerVersion) +# To enable jrl-cmakemodules compatibility with workspace we must define the two +# following lines +set(PROJECT_AUTO_RUN_FINALIZE FALSE) +set(PROJECT_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}) SET(PROJECT_USE_KEYWORD_LINK_LIBRARIES TRUE) SET(DOXYGEN_USE_TEMPLATE_CSS TRUE) @@ -50,8 +54,9 @@ SET(DOXYGEN_USE_TEMPLATE_CSS TRUE) # Need to be set before including base.cmake # ---------------------------------------------------- option(INSTALL_DOCUMENTATION "Generate and install the documentation" OFF) -option(HPP_FCL_TURN_ASSERT_INTO_EXCEPTION "Turn some critical HPP-FCL asserts to exception." FALSE) -option(HPP_FCL_ENABLE_LOGGING "Activate logging for warnings or error messages. Turned on by default in Debug." FALSE) +option(COAL_TURN_ASSERT_INTO_EXCEPTION "Turn some critical Coal asserts to exception." FALSE) +option(COAL_ENABLE_LOGGING "Activate logging for warnings or error messages. Turned on by default in Debug." FALSE) +option(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL "Make Coal retro-compatible with HPP-FCL." FALSE) # Check if the submodule cmake have been initialized set(JRL_CMAKE_MODULES "${CMAKE_CURRENT_LIST_DIR}/cmake") @@ -85,6 +90,7 @@ else() endif() include("${JRL_CMAKE_MODULES}/boost.cmake") +include("${JRL_CMAKE_MODULES}/python.cmake") include("${JRL_CMAKE_MODULES}/hpp.cmake") include("${JRL_CMAKE_MODULES}/apple.cmake") include("${JRL_CMAKE_MODULES}/ide.cmake") @@ -121,14 +127,16 @@ CMAKE_DEPENDENT_OPTION(GENERATE_PYTHON_STUBS "Generate the Python stubs associat ADD_PROJECT_DEPENDENCY(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.0.0") if(BUILD_PYTHON_INTERFACE) - FIND_PACKAGE(eigenpy 2.9.2 REQUIRED) + SET(PYTHON_COMPONENTS Interpreter Development NumPy) + FINDPYTHON(REQUIRED) + ADD_PROJECT_PRIVATE_DEPENDENCY(eigenpy 2.9.2 REQUIRED) endif() # Required dependencies SET_BOOST_DEFAULT_OPTIONS() EXPORT_BOOST_DEFAULT_OPTIONS() ADD_PROJECT_DEPENDENCY(Boost REQUIRED chrono thread date_time serialization filesystem) -if (HPP_FCL_ENABLE_LOGGING) +if (COAL_ENABLE_LOGGING) ADD_PROJECT_DEPENDENCY(Boost REQUIRED log) endif() if(BUILD_PYTHON_INTERFACE) @@ -146,31 +154,31 @@ endif() # Optional dependencies ADD_PROJECT_DEPENDENCY(octomap PKG_CONFIG_REQUIRES "octomap >= 1.6") if(octomap_FOUND) - SET(HPP_FCL_HAS_OCTOMAP TRUE) + SET(COAL_HAS_OCTOMAP TRUE) string(REPLACE "." ";" VERSION_LIST ${octomap_VERSION}) list(GET VERSION_LIST 0 OCTOMAP_MAJOR_VERSION) list(GET VERSION_LIST 1 OCTOMAP_MINOR_VERSION) list(GET VERSION_LIST 2 OCTOMAP_PATCH_VERSION) - message(STATUS "HPP-FCL uses Octomap") + message(STATUS "COAL uses Octomap") else() - SET(HPP_FCL_HAS_OCTOMAP FALSE) - message(STATUS "HPP-FCL does not use Octomap") + SET(COAL_HAS_OCTOMAP FALSE) + message(STATUS "COAL does not use Octomap") endif() -option(HPP_FCL_HAS_QHULL "use qhull library to compute convex hulls." FALSE) -if(HPP_FCL_HAS_QHULL) +option(COAL_HAS_QHULL "use qhull library to compute convex hulls." FALSE) +if(COAL_HAS_QHULL) find_package(Qhull COMPONENTS qhull_r qhullcpp) if(Qhull_FOUND) - set(HPP_FCL_USE_SYSTEM_QHULL TRUE) - message(STATUS "HPP-FCL uses system Qhull") + set(COAL_USE_SYSTEM_QHULL TRUE) + message(STATUS "COAL uses system Qhull") else() message(STATUS "Qhullcpp not found: it will be build from sources, if Qhull_r is found") file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/third-parties) execute_process(COMMAND ${CMAKE_COMMAND} -E create_symlink - ${CMAKE_SOURCE_DIR}/third-parties/qhull/src/libqhullcpp - ${CMAKE_CURRENT_BINARY_DIR}/third-parties/libqhullcpp + ${PROJECT_SOURCE_DIR}/third-parties/qhull/src/libqhullcpp + ${PROJECT_BINARY_DIR}/third-parties/libqhullcpp ) - set(Qhullcpp_PREFIX ${CMAKE_BINARY_DIR}/third-parties) + set(Qhullcpp_PREFIX ${PROJECT_BINARY_DIR}/third-parties) find_path(Qhull_r_INCLUDE_DIR NAMES libqhull_r/libqhull_r.h PATHS ${Qhull_PREFIX} @@ -180,7 +188,7 @@ if(HPP_FCL_HAS_QHULL) PATHS ${Qhull_PREFIX} ) if(NOT Qhull_r_LIBRARY) - message(FATAL_ERROR "Qhull_r not found, please install it or turn HPP_FCL_HAS_QHULL OFF") + message(FATAL_ERROR "Qhull_r not found, please install it or turn COAL_HAS_QHULL OFF") endif() endif() endif() @@ -188,133 +196,265 @@ endif() FIND_PACKAGE(assimp REQUIRED) SET(${PROJECT_NAME}_HEADERS - include/hpp/fcl/collision_data.h - include/hpp/fcl/BV/kIOS.h - include/hpp/fcl/BV/BV.h - include/hpp/fcl/BV/RSS.h - include/hpp/fcl/BV/OBBRSS.h - include/hpp/fcl/BV/BV_node.h - include/hpp/fcl/BV/AABB.h - include/hpp/fcl/BV/OBB.h - include/hpp/fcl/BV/kDOP.h - include/hpp/fcl/broadphase/broadphase.h - include/hpp/fcl/broadphase/broadphase_SSaP.h - include/hpp/fcl/broadphase/broadphase_SaP.h - include/hpp/fcl/broadphase/broadphase_bruteforce.h - include/hpp/fcl/broadphase/broadphase_collision_manager.h - include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h - include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h - include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h - include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h - include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h - include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h - include/hpp/fcl/broadphase/broadphase_interval_tree.h - include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h - include/hpp/fcl/broadphase/broadphase_spatialhash.h - include/hpp/fcl/broadphase/broadphase_callbacks.h - include/hpp/fcl/broadphase/default_broadphase_callbacks.h - include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h - include/hpp/fcl/broadphase/detail/hierarchy_tree.h - include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h - include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h - include/hpp/fcl/broadphase/detail/interval_tree.h - include/hpp/fcl/broadphase/detail/interval_tree_node.h - include/hpp/fcl/broadphase/detail/morton-inl.h - include/hpp/fcl/broadphase/detail/morton.h - include/hpp/fcl/broadphase/detail/node_base-inl.h - include/hpp/fcl/broadphase/detail/node_base.h - include/hpp/fcl/broadphase/detail/node_base_array-inl.h - include/hpp/fcl/broadphase/detail/node_base_array.h - include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h - include/hpp/fcl/broadphase/detail/simple_hash_table.h - include/hpp/fcl/broadphase/detail/simple_interval-inl.h - include/hpp/fcl/broadphase/detail/simple_interval.h - include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h - include/hpp/fcl/broadphase/detail/sparse_hash_table.h - include/hpp/fcl/broadphase/detail/spatial_hash-inl.h - include/hpp/fcl/broadphase/detail/spatial_hash.h - include/hpp/fcl/narrowphase/narrowphase.h - include/hpp/fcl/narrowphase/gjk.h - include/hpp/fcl/narrowphase/narrowphase_defaults.h - include/hpp/fcl/narrowphase/minkowski_difference.h - include/hpp/fcl/narrowphase/support_functions.h - include/hpp/fcl/shape/convex.h - include/hpp/fcl/shape/details/convex.hxx - include/hpp/fcl/shape/geometric_shape_to_BVH_model.h - include/hpp/fcl/shape/geometric_shapes.h - include/hpp/fcl/shape/geometric_shapes_traits.h - include/hpp/fcl/shape/geometric_shapes_utility.h - include/hpp/fcl/distance_func_matrix.h - include/hpp/fcl/collision.h - include/hpp/fcl/collision_func_matrix.h - include/hpp/fcl/contact_patch.h - include/hpp/fcl/contact_patch_func_matrix.h - include/hpp/fcl/contact_patch/contact_patch_solver.h - include/hpp/fcl/contact_patch/contact_patch_solver.hxx - include/hpp/fcl/distance.h - include/hpp/fcl/math/matrix_3f.h - include/hpp/fcl/math/vec_3f.h - include/hpp/fcl/math/types.h - include/hpp/fcl/math/transform.h - include/hpp/fcl/data_types.h - include/hpp/fcl/BVH/BVH_internal.h - include/hpp/fcl/BVH/BVH_model.h - include/hpp/fcl/BVH/BVH_front.h - include/hpp/fcl/BVH/BVH_utility.h - include/hpp/fcl/collision_object.h - include/hpp/fcl/collision_utility.h - include/hpp/fcl/hfield.h - include/hpp/fcl/fwd.hh - include/hpp/fcl/logging.h - include/hpp/fcl/mesh_loader/assimp.h - include/hpp/fcl/mesh_loader/loader.h - include/hpp/fcl/internal/BV_fitter.h - include/hpp/fcl/internal/BV_splitter.h - include/hpp/fcl/internal/shape_shape_func.h - include/hpp/fcl/internal/shape_shape_contact_patch_func.h - include/hpp/fcl/internal/intersect.h - include/hpp/fcl/internal/tools.h - include/hpp/fcl/internal/traversal_node_base.h - include/hpp/fcl/internal/traversal_node_bvh_shape.h - include/hpp/fcl/internal/traversal_node_bvhs.h - include/hpp/fcl/internal/traversal_node_hfield_shape.h - include/hpp/fcl/internal/traversal_node_setup.h - include/hpp/fcl/internal/traversal_node_shapes.h - include/hpp/fcl/internal/traversal_recurse.h - include/hpp/fcl/internal/traversal.h - include/hpp/fcl/serialization/fwd.h - include/hpp/fcl/serialization/serializer.h - include/hpp/fcl/serialization/archive.h - include/hpp/fcl/serialization/transform.h - include/hpp/fcl/serialization/AABB.h - include/hpp/fcl/serialization/BV_node.h - include/hpp/fcl/serialization/BV_splitter.h - include/hpp/fcl/serialization/BVH_model.h - include/hpp/fcl/serialization/collision_data.h - include/hpp/fcl/serialization/contact_patch.h - include/hpp/fcl/serialization/collision_object.h - include/hpp/fcl/serialization/convex.h - include/hpp/fcl/serialization/eigen.h - include/hpp/fcl/serialization/geometric_shapes.h - include/hpp/fcl/serialization/memory.h - include/hpp/fcl/serialization/OBB.h - include/hpp/fcl/serialization/RSS.h - include/hpp/fcl/serialization/OBBRSS.h - include/hpp/fcl/serialization/kIOS.h - include/hpp/fcl/serialization/kDOP.h - include/hpp/fcl/serialization/hfield.h - include/hpp/fcl/serialization/quadrilateral.h - include/hpp/fcl/serialization/triangle.h - include/hpp/fcl/timings.h + include/coal/collision_data.h + include/coal/BV/kIOS.h + include/coal/BV/BV.h + include/coal/BV/RSS.h + include/coal/BV/OBBRSS.h + include/coal/BV/BV_node.h + include/coal/BV/AABB.h + include/coal/BV/OBB.h + include/coal/BV/kDOP.h + include/coal/broadphase/broadphase.h + include/coal/broadphase/broadphase_SSaP.h + include/coal/broadphase/broadphase_SaP.h + include/coal/broadphase/broadphase_bruteforce.h + include/coal/broadphase/broadphase_collision_manager.h + include/coal/broadphase/broadphase_continuous_collision_manager-inl.h + include/coal/broadphase/broadphase_continuous_collision_manager.h + include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h + include/coal/broadphase/broadphase_dynamic_AABB_tree.h + include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h + include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h + include/coal/broadphase/broadphase_interval_tree.h + include/coal/broadphase/broadphase_spatialhash-inl.h + include/coal/broadphase/broadphase_spatialhash.h + include/coal/broadphase/broadphase_callbacks.h + include/coal/broadphase/default_broadphase_callbacks.h + include/coal/broadphase/detail/hierarchy_tree-inl.h + include/coal/broadphase/detail/hierarchy_tree.h + include/coal/broadphase/detail/hierarchy_tree_array-inl.h + include/coal/broadphase/detail/hierarchy_tree_array.h + include/coal/broadphase/detail/interval_tree.h + include/coal/broadphase/detail/interval_tree_node.h + include/coal/broadphase/detail/morton-inl.h + include/coal/broadphase/detail/morton.h + include/coal/broadphase/detail/node_base-inl.h + include/coal/broadphase/detail/node_base.h + include/coal/broadphase/detail/node_base_array-inl.h + include/coal/broadphase/detail/node_base_array.h + include/coal/broadphase/detail/simple_hash_table-inl.h + include/coal/broadphase/detail/simple_hash_table.h + include/coal/broadphase/detail/simple_interval-inl.h + include/coal/broadphase/detail/simple_interval.h + include/coal/broadphase/detail/sparse_hash_table-inl.h + include/coal/broadphase/detail/sparse_hash_table.h + include/coal/broadphase/detail/spatial_hash-inl.h + include/coal/broadphase/detail/spatial_hash.h + include/coal/narrowphase/narrowphase.h + include/coal/narrowphase/gjk.h + include/coal/narrowphase/narrowphase_defaults.h + include/coal/narrowphase/minkowski_difference.h + include/coal/narrowphase/support_functions.h + include/coal/shape/convex.h + include/coal/shape/details/convex.hxx + include/coal/shape/geometric_shape_to_BVH_model.h + include/coal/shape/geometric_shapes.h + include/coal/shape/geometric_shapes_traits.h + include/coal/shape/geometric_shapes_utility.h + include/coal/distance_func_matrix.h + include/coal/collision.h + include/coal/collision_func_matrix.h + include/coal/contact_patch.h + include/coal/contact_patch_func_matrix.h + include/coal/contact_patch/contact_patch_solver.h + include/coal/contact_patch/contact_patch_solver.hxx + include/coal/distance.h + include/coal/math/matrix_3f.h + include/coal/math/vec_3f.h + include/coal/math/types.h + include/coal/math/transform.h + include/coal/data_types.h + include/coal/BVH/BVH_internal.h + include/coal/BVH/BVH_model.h + include/coal/BVH/BVH_front.h + include/coal/BVH/BVH_utility.h + include/coal/collision_object.h + include/coal/collision_utility.h + include/coal/hfield.h + include/coal/fwd.hh + include/coal/logging.h + include/coal/mesh_loader/assimp.h + include/coal/mesh_loader/loader.h + include/coal/internal/BV_fitter.h + include/coal/internal/BV_splitter.h + include/coal/internal/shape_shape_func.h + include/coal/internal/shape_shape_contact_patch_func.h + include/coal/internal/intersect.h + include/coal/internal/tools.h + include/coal/internal/traversal_node_base.h + include/coal/internal/traversal_node_bvh_shape.h + include/coal/internal/traversal_node_bvhs.h + include/coal/internal/traversal_node_hfield_shape.h + include/coal/internal/traversal_node_setup.h + include/coal/internal/traversal_node_shapes.h + include/coal/internal/traversal_recurse.h + include/coal/internal/traversal.h + include/coal/serialization/fwd.h + include/coal/serialization/serializer.h + include/coal/serialization/archive.h + include/coal/serialization/transform.h + include/coal/serialization/AABB.h + include/coal/serialization/BV_node.h + include/coal/serialization/BV_splitter.h + include/coal/serialization/BVH_model.h + include/coal/serialization/collision_data.h + include/coal/serialization/contact_patch.h + include/coal/serialization/collision_object.h + include/coal/serialization/convex.h + include/coal/serialization/eigen.h + include/coal/serialization/geometric_shapes.h + include/coal/serialization/memory.h + include/coal/serialization/OBB.h + include/coal/serialization/RSS.h + include/coal/serialization/OBBRSS.h + include/coal/serialization/kIOS.h + include/coal/serialization/kDOP.h + include/coal/serialization/hfield.h + include/coal/serialization/quadrilateral.h + include/coal/serialization/triangle.h + include/coal/timings.h ) -IF(HPP_FCL_HAS_OCTOMAP) - LIST(APPEND ${PROJECT_NAME}_HEADERS +if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) + SET(HPP_FCL_BACKWARD_COMPATIBILITY_HEADERS + include/hpp/fcl/broadphase/broadphase_bruteforce.h + include/hpp/fcl/broadphase/broadphase_callbacks.h + include/hpp/fcl/broadphase/broadphase_collision_manager.h + include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h + include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h + include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h + include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h + include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h + include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h + include/hpp/fcl/broadphase/broadphase.h + include/hpp/fcl/broadphase/broadphase_interval_tree.h + include/hpp/fcl/broadphase/broadphase_SaP.h + include/hpp/fcl/broadphase/broadphase_spatialhash.h + include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h + include/hpp/fcl/broadphase/broadphase_SSaP.h + include/hpp/fcl/broadphase/default_broadphase_callbacks.h + include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h + include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h + include/hpp/fcl/broadphase/detail/hierarchy_tree.h + include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h + include/hpp/fcl/broadphase/detail/interval_tree.h + include/hpp/fcl/broadphase/detail/interval_tree_node.h + include/hpp/fcl/broadphase/detail/morton.h + include/hpp/fcl/broadphase/detail/morton-inl.h + include/hpp/fcl/broadphase/detail/node_base_array.h + include/hpp/fcl/broadphase/detail/node_base_array-inl.h + include/hpp/fcl/broadphase/detail/node_base.h + include/hpp/fcl/broadphase/detail/node_base-inl.h + include/hpp/fcl/broadphase/detail/simple_hash_table.h + include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h + include/hpp/fcl/broadphase/detail/simple_interval.h + include/hpp/fcl/broadphase/detail/simple_interval-inl.h + include/hpp/fcl/broadphase/detail/sparse_hash_table.h + include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h + include/hpp/fcl/broadphase/detail/spatial_hash.h + include/hpp/fcl/broadphase/detail/spatial_hash-inl.h + include/hpp/fcl/BV/AABB.h + include/hpp/fcl/BV/BV.h + include/hpp/fcl/BV/BV_node.h + include/hpp/fcl/BVH/BVH_front.h + include/hpp/fcl/BVH/BVH_internal.h + include/hpp/fcl/BVH/BVH_model.h + include/hpp/fcl/BVH/BVH_utility.h + include/hpp/fcl/BV/kDOP.h + include/hpp/fcl/BV/kIOS.h + include/hpp/fcl/BV/OBB.h + include/hpp/fcl/BV/OBBRSS.h + include/hpp/fcl/BV/RSS.h + include/hpp/fcl/coal.hpp + include/hpp/fcl/collision_data.h + include/hpp/fcl/collision_func_matrix.h + include/hpp/fcl/collision.h + include/hpp/fcl/collision_object.h + include/hpp/fcl/collision_utility.h + include/hpp/fcl/config.hh + include/hpp/fcl/contact_patch/contact_patch_solver.h + include/hpp/fcl/contact_patch/contact_patch_solver.hxx + include/hpp/fcl/contact_patch_func_matrix.h + include/hpp/fcl/contact_patch.h + include/hpp/fcl/data_types.h + include/hpp/fcl/deprecated.hh + include/hpp/fcl/distance_func_matrix.h + include/hpp/fcl/distance.h + include/hpp/fcl/fwd.hh + include/hpp/fcl/hfield.h + include/hpp/fcl/internal/BV_fitter.h + include/hpp/fcl/internal/BV_splitter.h + include/hpp/fcl/internal/intersect.h + include/hpp/fcl/internal/shape_shape_contact_patch_func.h + include/hpp/fcl/internal/shape_shape_func.h + include/hpp/fcl/internal/tools.h + include/hpp/fcl/internal/traversal.h + include/hpp/fcl/internal/traversal_node_base.h + include/hpp/fcl/internal/traversal_node_bvhs.h + include/hpp/fcl/internal/traversal_node_bvh_shape.h + include/hpp/fcl/internal/traversal_node_hfield_shape.h + include/hpp/fcl/internal/traversal_node_setup.h + include/hpp/fcl/internal/traversal_node_shapes.h + include/hpp/fcl/internal/traversal_recurse.h + include/hpp/fcl/internal/traversal_node_octree.h + include/hpp/fcl/logging.h + include/hpp/fcl/math/matrix_3f.h + include/hpp/fcl/math/transform.h + include/hpp/fcl/math/types.h + include/hpp/fcl/math/vec_3f.h + include/hpp/fcl/mesh_loader/assimp.h + include/hpp/fcl/mesh_loader/loader.h + include/hpp/fcl/narrowphase/gjk.h + include/hpp/fcl/narrowphase/minkowski_difference.h + include/hpp/fcl/narrowphase/narrowphase_defaults.h + include/hpp/fcl/narrowphase/narrowphase.h + include/hpp/fcl/narrowphase/support_functions.h include/hpp/fcl/octree.h + include/hpp/fcl/serialization/AABB.h + include/hpp/fcl/serialization/archive.h + include/hpp/fcl/serialization/BVH_model.h + include/hpp/fcl/serialization/BV_node.h + include/hpp/fcl/serialization/BV_splitter.h + include/hpp/fcl/serialization/collision_data.h + include/hpp/fcl/serialization/collision_object.h + include/hpp/fcl/serialization/contact_patch.h + include/hpp/fcl/serialization/convex.h + include/hpp/fcl/serialization/eigen.h + include/hpp/fcl/serialization/fwd.h + include/hpp/fcl/serialization/geometric_shapes.h + include/hpp/fcl/serialization/hfield.h + include/hpp/fcl/serialization/kDOP.h + include/hpp/fcl/serialization/kIOS.h + include/hpp/fcl/serialization/memory.h + include/hpp/fcl/serialization/OBB.h + include/hpp/fcl/serialization/OBBRSS.h include/hpp/fcl/serialization/octree.h - include/hpp/fcl/internal/traversal_node_octree.h + include/hpp/fcl/serialization/quadrilateral.h + include/hpp/fcl/serialization/RSS.h + include/hpp/fcl/serialization/serializer.h + include/hpp/fcl/serialization/transform.h + include/hpp/fcl/serialization/triangle.h + include/hpp/fcl/shape/convex.h + include/hpp/fcl/shape/details/convex.hxx + include/hpp/fcl/shape/geometric_shapes.h + include/hpp/fcl/shape/geometric_shapes_traits.h + include/hpp/fcl/shape/geometric_shapes_utility.h + include/hpp/fcl/shape/geometric_shape_to_BVH_model.h + include/hpp/fcl/timings.h + include/hpp/fcl/warning.hh + ) + LIST(APPEND ${PROJECT_NAME}_HEADERS ${HPP_FCL_BACKWARD_COMPATIBILITY_HEADERS}) + HEADER_INSTALL(COMPONENT hpp-fcl-compatibility ${HPP_FCL_BACKWARD_COMPATIBILITY_HEADERS}) +endif() + +IF(COAL_HAS_OCTOMAP) + LIST(APPEND ${PROJECT_NAME}_HEADERS + include/coal/octree.h + include/coal/serialization/octree.h + include/coal/internal/traversal_node_octree.h ) -ENDIF(HPP_FCL_HAS_OCTOMAP) +ENDIF(COAL_HAS_OCTOMAP) add_subdirectory(doc) add_subdirectory(src) @@ -325,12 +465,12 @@ if(BUILD_TESTING) add_subdirectory(test) endif(BUILD_TESTING) -pkg_config_append_libs("hpp-fcl") -IF(HPP_FCL_HAS_OCTOMAP) +pkg_config_append_libs("coal") +IF(COAL_HAS_OCTOMAP) # FCL_HAVE_OCTOMAP kept for backward compatibility reasons. PKG_CONFIG_APPEND_CFLAGS( - "-DHPP_FCL_HAS_OCTOMAP -DHPP_FCL_HAVE_OCTOMAP -DFCL_HAVE_OCTOMAP -DOCTOMAP_MAJOR_VERSION=${OCTOMAP_MAJOR_VERSION} -DOCTOMAP_MINOR_VERSION=${OCTOMAP_MINOR_VERSION} -DOCTOMAP_PATCH_VERSION=${OCTOMAP_PATCH_VERSION}") -ENDIF(HPP_FCL_HAS_OCTOMAP) + "-DCOAL_HAS_OCTOMAP -DCOAL_HAVE_OCTOMAP -DFCL_HAVE_OCTOMAP -DOCTOMAP_MAJOR_VERSION=${OCTOMAP_MAJOR_VERSION} -DOCTOMAP_MINOR_VERSION=${OCTOMAP_MINOR_VERSION} -DOCTOMAP_PATCH_VERSION=${OCTOMAP_PATCH_VERSION}") +ENDIF(COAL_HAS_OCTOMAP) # Install catkin package.xml INSTALL(FILES package.xml DESTINATION share/${PROJECT_NAME}) @@ -341,3 +481,17 @@ file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/share/${PROJECT_NAME}/hook/ament_prefix_p install(FILES ${CMAKE_CURRENT_BINARY_DIR}/share/${PROJECT_NAME}/hook/ament_prefix_path.dsv DESTINATION share/${PROJECT_NAME}/hook) file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/share/${PROJECT_NAME}/hook/python_path.dsv "prepend-non-duplicate;PYTHONPATH;${PYTHON_SITELIB}") install(FILES ${CMAKE_CURRENT_BINARY_DIR}/share/${PROJECT_NAME}/hook/python_path.dsv DESTINATION share/${PROJECT_NAME}/hook) + +if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) + include(CMakePackageConfigHelpers) + write_basic_package_version_file(hpp-fclConfigVersion.cmake + VERSION 3.0.0 + COMPATIBILITY AnyNewerVersion) + install(FILES hpp-fclConfig.cmake ${CMAKE_CURRENT_BINARY_DIR}/hpp-fclConfigVersion.cmake + DESTINATION lib/cmake/hpp-fcl + COMPONENT hpp-fcl-compatibility) + include("${JRL_CMAKE_MODULES}/install-helpers.cmake") + add_install_target(NAME hpp-fcl-compatibility COMPONENT hpp-fcl-compatibility) +endif() + +setup_project_finalize() diff --git a/README.md b/README.md index d8a6d1af4..88e27ccac 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# HPP-FCL — An extension of the Flexible Collision Library +# Coal — An extension of the Flexible Collision Library

Pipeline status @@ -11,16 +11,16 @@ ruff

-[FCL](https://github.com/flexible-collision-library/fcl) was forked in 2015. -Since then, a large part of the code has been rewritten or removed (for the unused and untested part). -The broad phase was reintroduced by [Justin Carpentier](https://github.com/jcarpent) in 2022 based on the FCL version 0.7.0. +[FCL](https://github.com/flexible-collision-library/fcl) was forked in 2015, creating a new project called HPP-FCL. +Since then, a large part of the code has been rewritten or removed (for the unused and untested part), and new features have been developped (see below). +Due to these major changes, it was decided in 2024 to rename the HPP-FCL project to **Coal**. -If you use **HPP-FCL** in your projects and research papers, we would appreciate it if you'd [cite it](https://raw.githubusercontent.com/humanoid-path-planner/hpp-fcl/devel/CITATION.bib). +If you use **Coal** in your projects and research papers, we would appreciate it if you'd [cite it](https://raw.githubusercontent.com/coal-library/coal/devel/CITATION.bib). ## New features Compared to the original [FCL](https://github.com/flexible-collision-library/fcl) library, the main new features are: -- a dedicated and efficient implementation of the GJK algorithm (we do not rely anymore on [libccd](https://github.com/danfis/libccd)) +- dedicated and efficient implementations of the GJK and the EPA algorithms (we do not rely on [libccd](https://github.com/danfis/libccd)) - the support of safety margins for collision detection - an accelerated version of collision detection *à la Nesterov*, which leads to increased performances (up to a factor of 2). More details are available in this [paper](https://hal.archives-ouvertes.fr/hal-03662157/) - the computation of a lower bound of the distance between two objects when collision checking is performed, and no collision is found @@ -30,13 +30,15 @@ Compared to the original [FCL](https://github.com/flexible-collision-library/fcl - efficient computation of **contact points** and **contact patches** between objects - full support of object serialization via Boost.Serialization -This project is now used in many robotics frameworks such as [Pinocchio](https://github.com/stack-of-tasks/pinocchio), an open-source software that implements efficient and versatile rigid body dynamics algorithms, the [Humanoid Path Planner](https://humanoid-path-planner.github.io/hpp-doc), an open-source software for Motion and Manipulation Planning. **HPP-FCL** has also been recently used to develop [Simple](https://github.com/Simple-Robotics/Simple), a new (differentiable) and efficient simulator for robotics and beyond. +Note: the broad phase was reintroduced by [Justin Carpentier](https://github.com/jcarpent) in 2022, based on the FCL version 0.7.0. + +This project is now used in many robotics frameworks such as [Pinocchio](https://github.com/stack-of-tasks/pinocchio), an open-source software that implements efficient and versatile rigid body dynamics algorithms, the [Humanoid Path Planner](https://humanoid-path-planner.github.io/hpp-doc), an open-source software for Motion and Manipulation Planning. **Coal** has also been recently used to develop [Simple](https://github.com/Simple-Robotics/Simple), a new (differentiable) and efficient simulator for robotics and beyond. ## A high-performance library -Unlike the original FCL library, HPP-FCL implements the well-established [GJK algorithm](https://en.wikipedia.org/wiki/Gilbert%E2%80%93Johnson%E2%80%93Keerthi_distance_algorithm) and [its variants](https://hal.archives-ouvertes.fr/hal-03662157/) for collision detection and distance computation. These implementations lead to state-of-the-art performances, as depicted by the figures below. +Unlike the original FCL library, Coal implements the well-established [GJK algorithm](https://en.wikipedia.org/wiki/Gilbert%E2%80%93Johnson%E2%80%93Keerthi_distance_algorithm) and [its variants](https://hal.archives-ouvertes.fr/hal-03662157/) for collision detection and distance computation. These implementations lead to state-of-the-art performances, as depicted by the figures below. -On the one hand, we have benchmarked HPP-FCL against major software alternatives of the state of the art: +On the one hand, we have benchmarked Coal against major software alternatives of the state of the art: 1. the [Bullet simulator](https://github.com/bulletphysics/bullet3), 2. the original [FCL library](https://github.com/flexible-collision-library/fcl) (used in the [Drake framework]()), 3. the [libccd library](https://github.com/danfis/libccd) (used in [MuJoCo](http://mujoco.org/)). @@ -45,13 +47,13 @@ The results are depicted in the following figure, which notably shows that the a Please notice that the y-axis is in log scale.

- HPP-FCL vs the rest of the world + Coal vs the rest of the world

-On the other hand, why do we care about dedicated collision detection solvers like GJK for the narrow phase? Why can't we simply formulate the collision detection problem as a quadratic problem and call an off-the-shelf optimization solver like [ProxQP](https://github.com/Simple-Robotics/proxsuite))? Here is why. +On the other hand, why do we care about dedicated collision detection solvers like GJK for the narrow phase? Why can't we simply formulate the collision detection problem as a quadratic problem and call an off-the-shelf optimization solver like [ProxQP](https://github.com/Simple-Robotics/proxsuite))? Here is why:

- HPP-FCL vs generic QP solvers + Coal vs generic QP solvers

One can observe that GJK-based approaches largely outperform solutions based on classic optimization solvers (e.g., QP solver like [ProxQP](https://github.com/Simple-Robotics/proxsuite)), notably for large geometries composed of tens or hundreds of vertices. @@ -67,16 +69,16 @@ One can observe that GJK-based approaches largely outperform solutions based on - [ocs2](https://github.com/leggedrobotics/ocs2) A toolbox for Optimal Control for Switched Systems (OCS2) ## C++ example -Both the C++ library and the python bindings can be installed as simply as `conda -c conda-forge install hpp-fcl`. +Both the C++ library and the python bindings can be installed as simply as `conda -c conda-forge install coal`. The `.so` library, include files and python bindings will then be installed under `$CONDA_PREFIX/lib`, `$CONDA_PREFIX/include` and `$CONDA_PREFIX/lib/python3.XX/site-packages`. -Here is an example of using HPP-FCL in C++: +Here is an example of using Coal in C++: ```cpp -#include "hpp/fcl/math/transform.h" -#include "hpp/fcl/mesh_loader/loader.h" -#include "hpp/fcl/BVH/BVH_model.h" -#include "hpp/fcl/collision.h" -#include "hpp/fcl/collision_data.h" +#include "coal/math/transform.h" +#include "coal/mesh_loader/loader.h" +#include "coal/BVH/BVH_model.h" +#include "coal/collision.h" +#include "coal/collision_data.h" #include #include @@ -94,29 +96,29 @@ Here is an example of using HPP-FCL in C++: // GJK or EPA can be called with this object. // Consequently, after creating the BVH structure from the point cloud, this function // also computes its convex hull. -std::shared_ptr loadConvexMesh(const std::string& file_name) { - hpp::fcl::NODE_TYPE bv_type = hpp::fcl::BV_AABB; - hpp::fcl::MeshLoader loader(bv_type); - hpp::fcl::BVHModelPtr_t bvh = loader.load(file_name); +std::shared_ptr loadConvexMesh(const std::string& file_name) { + coal::NODE_TYPE bv_type = coal::BV_AABB; + coal::MeshLoader loader(bv_type); + coal::BVHModelPtr_t bvh = loader.load(file_name); bvh->buildConvexHull(true, "Qt"); return bvh->convex; } int main() { - // Create the hppfcl shapes. - // Hppfcl supports many primitive shapes: boxes, spheres, capsules, cylinders, ellipsoids, cones, planes, + // Create the coal shapes. + // Coal supports many primitive shapes: boxes, spheres, capsules, cylinders, ellipsoids, cones, planes, // halfspace and convex meshes (i.e. convex hulls of clouds of points). // It also supports BVHs (bounding volumes hierarchies), height-fields and octrees. - std::shared_ptr shape1 = std::make_shared(0.7, 1.0, 0.8); - std::shared_ptr shape2 = loadConvexMesh("../path/to/mesh/file.obj"); + std::shared_ptr shape1 = std::make_shared(0.7, 1.0, 0.8); + std::shared_ptr shape2 = loadConvexMesh("../path/to/mesh/file.obj"); // Define the shapes' placement in 3D space - hpp::fcl::Transform3f T1; - T1.setQuatRotation(hpp::fcl::Quaternion3f::UnitRandom()); - T1.setTranslation(hpp::fcl::Vec3f::Random()); - hpp::fcl::Transform3f T2 = hpp::fcl::Transform3f::Identity(); - T2.setQuatRotation(hpp::fcl::Quaternion3f::UnitRandom()); - T2.setTranslation(hpp::fcl::Vec3f::Random()); + coal::Transform3s T1; + T1.setQuatRotation(coal::Quaternion3f::UnitRandom()); + T1.setTranslation(coal::Vec3s::Random()); + coal::Transform3s T2 = coal::Transform3s::Identity(); + T2.setQuatRotation(coal::Quaternion3f::UnitRandom()); + T2.setTranslation(coal::Vec3s::Random()); // Define collision requests and results. // @@ -127,19 +129,19 @@ int main() { // Setting a positive security margin can be usefull in motion planning, // i.e to prevent shapes from getting too close to one another. // In physics simulation, allowing a negative security margin may be usefull to stabilize the simulation. - hpp::fcl::CollisionRequest col_req; + coal::CollisionRequest col_req; col_req.security_margin = 1e-1; // A collision result stores the result of the collision test (signed distance between the shapes, // witness points location, normal etc.) - hpp::fcl::CollisionResult col_res; + coal::CollisionResult col_res; // Collision call - hpp::fcl::collide(shape1.get(), T1, shape2.get(), T2, col_req, col_res); + coal::collide(shape1.get(), T1, shape2.get(), T2, col_req, col_res); // We can access the collision result once it has been populated std::cout << "Collision? " << col_res.isCollision() << "\n"; if (col_res.isCollision()) { - hpp::fcl::Contact contact = col_res.getContact(0); + coal::Contact contact = col_res.getContact(0); // The penetration depth does **not** take into account the security margin. // Consequently, the penetration depth is the true signed distance which separates the shapes. // To have the distance which takes into account the security margin, we can simply add the two together. @@ -158,47 +160,47 @@ int main() { ``` ## Python example -Here is the C++ example from above translated in python using HPP-FCL's python bindings: +Here is the C++ example from above translated in python using the python bindings of Coal: ```python import numpy as np -import hppfcl +import coal # Optional: # The Pinocchio library is a rigid body algorithms library and has a handy SE3 module. # It can be installed as simply as `conda -c conda-forge install pinocchio`. -# Installing pinocchio also installs hpp-fcl. +# Installing pinocchio also installs coal. import pinocchio as pin def loadConvexMesh(file_name: str): - loader = hppfcl.MeshLoader() - bvh: hppfcl.BVHModelBase = loader.load(file_name) + loader = coal.MeshLoader() + bvh: coal.BVHModelBase = loader.load(file_name) bvh.buildConvexHull(True, "Qt") return bvh.convex if __name__ == "__main__": - # Create hppfcl shapes - shape1 = hppfcl.Ellipsoid(0.7, 1.0, 0.8) + # Create coal shapes + shape1 = coal.Ellipsoid(0.7, 1.0, 0.8) shape2 = loadConvexMesh("../path/to/mesh/file.obj") # Define the shapes' placement in 3D space - T1 = hppfcl.Transform3f() + T1 = coal.Transform3s() T1.setTranslation(pin.SE3.Random().translation) T1.setRotation(pin.SE3.Random().rotation) - T2 = hppfcl.Transform3f(); + T2 = coal.Transform3s(); # Using np arrays also works T1.setTranslation(np.random.rand(3)) T2.setRotation(pin.SE3.Random().rotation) # Define collision requests and results - col_req = hppfcl.CollisionRequest() - col_res = hppfcl.CollisionResult() + col_req = coal.CollisionRequest() + col_res = coal.CollisionResult() # Collision call - hppfcl.collide(shape1, T1, shape2, T2, col_req, col_res) + coal.collide(shape1, T1, shape2, T2, col_req, col_res) # Accessing the collision result once it has been populated print("Is collision? ", {col_res.isCollision()}) if col_res.isCollision(): - contact: hppfcl.Contact = col_res.getContact(0) + contact: coal.Contact = col_res.getContact(0) print("Penetration depth: ", contact.penetration_depth) print("Distance between the shapes including the security margin: ", contact.penetration_depth + col_req.security_margin) print("Witness point shape1: ", contact.getNearestPoint1()) @@ -211,4 +213,4 @@ if __name__ == "__main__": ## Acknowledgments -The development of **HPP-FCL** is actively supported by the [Gepetto team](http://projects.laas.fr/gepetto/) [@LAAS-CNRS](http://www.laas.fr), the [Willow team](https://www.di.ens.fr/willow/) [@INRIA](http://www.inria.fr) and, to some extend, [Eureka Robotics](https://eurekarobotics.com/). +The development of **Coal** is actively supported by the [Gepetto team](http://projects.laas.fr/gepetto/) [@LAAS-CNRS](http://www.laas.fr), the [Willow team](https://www.di.ens.fr/willow/) [@INRIA](http://www.inria.fr) and, to some extend, [Eureka Robotics](https://eurekarobotics.com/). diff --git a/cmake b/cmake index baf48a3ec..f1f95f942 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit baf48a3ecb96f8dcad616aec006d43cce3307e19 +Subproject commit f1f95f942fabd3d4dc7e39aa7f1c20a5b0735165 diff --git a/doc/gjk.py b/doc/gjk.py index b69bcc2b3..bee0d547a 100644 --- a/doc/gjk.py +++ b/doc/gjk.py @@ -444,34 +444,34 @@ def printOrder(order, indent="", start=True, file=sys.stdout, curTests=[]): for v in "abcd": print( indent - + "const Vec3f& {} (current.vertex[{}]->w);".format(v.upper(), v), + + "const Vec3s& {} (current.vertex[{}]->w);".format(v.upper(), v), file=file, ) - print(indent + "const FCL_REAL aa = A.squaredNorm();".format(), file=file) + print(indent + "const CoalScalar aa = A.squaredNorm();".format(), file=file) for v in "dcb": for m in "abcd": if m <= v: print( indent - + "const FCL_REAL {0}{1} = {2}.dot({3});".format( + + "const CoalScalar {0}{1} = {2}.dot({3});".format( v, m, v.upper(), m.upper() ), file=file, ) else: print( - indent + "const FCL_REAL& {0}{1} = {1}{0};".format(v, m), + indent + "const CoalScalar& {0}{1} = {1}{0};".format(v, m), file=file, ) - print(indent + "const FCL_REAL {0}a_aa = {0}a - aa;".format(v), file=file) + print(indent + "const CoalScalar {0}a_aa = {0}a - aa;".format(v), file=file) for l0, l1 in zip("bcd", "cdb"): print( - indent + "const FCL_REAL {0}a_{1}a = {0}a - {1}a;".format(l0, l1), + indent + "const CoalScalar {0}a_{1}a = {0}a - {1}a;".format(l0, l1), file=file, ) for v in "bc": print( - indent + "const Vec3f a_cross_{0} = A.cross({1});".format(v, v.upper()), + indent + "const Vec3s a_cross_{0} = A.cross({1});".format(v, v.upper()), file=file, ) print("", file=file) diff --git a/doc/images/hpp-fcl-performances.jpg b/doc/images/coal-performances.jpg similarity index 100% rename from doc/images/hpp-fcl-performances.jpg rename to doc/images/coal-performances.jpg diff --git a/doc/images/hpp-fcl-vs-the-rest-of-the-world.pdf b/doc/images/coal-vs-the-rest-of-the-world.pdf similarity index 100% rename from doc/images/hpp-fcl-vs-the-rest-of-the-world.pdf rename to doc/images/coal-vs-the-rest-of-the-world.pdf diff --git a/doc/images/hpp-fcl-vs-the-rest-of-the-world.png b/doc/images/coal-vs-the-rest-of-the-world.png similarity index 100% rename from doc/images/hpp-fcl-vs-the-rest-of-the-world.png rename to doc/images/coal-vs-the-rest-of-the-world.png diff --git a/doc/mesh-mesh-collision-call.dot b/doc/mesh-mesh-collision-call.dot index 55c455482..e70206145 100644 --- a/doc/mesh-mesh-collision-call.dot +++ b/doc/mesh-mesh-collision-call.dot @@ -4,35 +4,35 @@ digraph CD { compound=true size = 11.7 - "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3f& tf1, const CollisionGeometry* o2,\nconst Transform3f& tf2, const CollisionRequest& request,\nCollisionResult& result)" [shape = box] - "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" [shape = box] - "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" [shape = box] - "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, FCL_REAL& sqrDistLowerBound)" [shape = box] - "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)" [shape = box] - "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" [shape = box] - "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" [shape = box] - "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" [shape = box] + "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3s& tf1, const CollisionGeometry* o2,\nconst Transform3s& tf2, const CollisionRequest& request,\nCollisionResult& result)" [shape = box] + "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box] + "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box] + "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box] + "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box] + "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box] + "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] + "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" [shape = box] - "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)" [shape = box] + "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" [shape = box] "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" [shape = box] - "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" [shape = box] - "MeshCollisionTraversalNode::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" [shape = box] - "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n FCL_REAL& squaredLowerBoundDistance)" [shape = box] + "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] + "MeshCollisionTraversalNode::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] + "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)" [shape = box] - "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3f& tf1, const CollisionGeometry* o2,\nconst Transform3f& tf2, const CollisionRequest& request,\nCollisionResult& result)" -> "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" - "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n FCL_REAL& squaredLowerBoundDistance)" - "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, FCL_REAL& sqrDistLowerBound)" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n FCL_REAL& squaredLowerBoundDistance)" - "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, FCL_REAL& sqrDistLowerBound)" + "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3s& tf1, const CollisionGeometry* o2,\nconst Transform3s& tf2, const CollisionRequest& request,\nCollisionResult& result)" -> "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" + "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)" + "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)" + "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)"-> "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" - "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" -> "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)" - "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)" -> "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" - "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)" -> "MeshCollisionTraversalNode::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" - "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)" - "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" -"bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)" - "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" -> "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" [color=red] - "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" -> "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" [color = red] - "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" - "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" - "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" -> "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" + "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" -> "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" + "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" -> "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" + "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" -> "MeshCollisionTraversalNode::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" + "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" + "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" +"bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" + "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" -> "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [color=red] + "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" -> "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [color = red] + "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" + "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" + "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" -> "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" } \ No newline at end of file diff --git a/doc/shape-mesh-collision-call.dot b/doc/shape-mesh-collision-call.dot index 839f575a9..2a16e9f98 100644 --- a/doc/shape-mesh-collision-call.dot +++ b/doc/shape-mesh-collision-call.dot @@ -4,33 +4,33 @@ digraph CD { compound=true size = 11.7 - "BVHShapeCollider::collide\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" [shape = box] - "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" [shape = box] + "BVHShapeCollider::collide\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" [shape = box] + "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" [shape = box] "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" [shape = box] "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" [shape = box] - "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" [shape = box] - "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" [shape = box] - "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" [shape = box] - "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box] - "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box] - "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound)\nBV/OBBRSS.cpp" [shape = box] - "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)\nBV/OBB.cpp" [shape = box] - "bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,\nconst Vec3f& a, const Vec3f& b,\nconst CollisionRequest& request,\nFCL_REAL& squaredLowerBoundDistance)\nBV/OBB.cpp" [shape = box] - "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" [shape = box] - "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3f& tf,\nconst Vec3f& P1, const Vec3f& P2, const Vec3f& P3,\nFCL_REAL* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h:156" [shape = box] + "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" [shape = box] + "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" [shape = box] + "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" [shape = box] + "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box] + "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box] + "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" [shape = box] + "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" [shape = box] + "bool obbDisjointAndLowerBoundDistance (const Matrix3s& B, const Vec3s& T,\nconst Vec3s& a, const Vec3s& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" [shape = box] + "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" [shape = box] + "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3s& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156" [shape = box] - "BVHShapeCollider::collide\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" -> "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" - "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" -> "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" + "BVHShapeCollider::collide\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" -> "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" + "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" -> "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" - "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" - "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" - "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" - "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" - "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color=red] - "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color = red] - "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound)\nBV/OBBRSS.cpp" - "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound)\nBV/OBBRSS.cpp" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)\nBV/OBB.cpp" - "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)\nBV/OBB.cpp" -> "bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,\nconst Vec3f& a, const Vec3f& b,\nconst CollisionRequest& request,\nFCL_REAL& squaredLowerBoundDistance)\nBV/OBB.cpp" - "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" - "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" -> "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3f& tf,\nconst Vec3f& P1, const Vec3f& P2, const Vec3f& P3,\nFCL_REAL* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h:156" + "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" + "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" + "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" + "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" + "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color=red] + "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color = red] + "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" + "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" + "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" -> "bool obbDisjointAndLowerBoundDistance (const Matrix3s& B, const Vec3s& T,\nconst Vec3s& a, const Vec3s& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" + "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" + "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" -> "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3s& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156" } diff --git a/doc/shape-shape-collision-call.dot b/doc/shape-shape-collision-call.dot index 6690f4eb3..58d666331 100644 --- a/doc/shape-shape-collision-call.dot +++ b/doc/shape-shape-collision-call.dot @@ -4,16 +4,16 @@ digraph CD { compound=true size = 11.7 - "template\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" [shape = box] - "template\nFCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" [shape = box] + "template\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" [shape = box] + "template\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" [shape = box] "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" [shape = box] "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" [shape = box] "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" [shape = box] - "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nFCL_REAL* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h" [shape = box] + "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h" [shape = box] - "template\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" -> "template\nFCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" - "template\nFCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" -> "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" + "template\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" -> "template\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" + "template\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" -> "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" -> "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" -> "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" - "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" -> "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nFCL_REAL* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h" + "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" -> "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h" } diff --git a/hpp-fclConfig.cmake b/hpp-fclConfig.cmake new file mode 100644 index 000000000..1e2664285 --- /dev/null +++ b/hpp-fclConfig.cmake @@ -0,0 +1,14 @@ +# This file provide bacward compatiblity for `find_package(hpp-fcl)`. + +message(WARNING "Please update your CMake from 'hpp-fcl' to 'coal'") + +find_package(coal REQUIRED) + +if(NOT TARGET hpp-fcl::hpp-fcl) + add_library(hpp-fcl::hpp-fcl SHARED IMPORTED) + target_link_libraries(hpp-fcl::hpp-fcl INTERFACE coal::coal) + get_property(_cfg TARGET coal::coal PROPERTY IMPORTED_CONFIGURATIONS) + get_property(_loc TARGET coal::coal PROPERTY "IMPORTED_LOCATION_${_cfg}") + set_property(TARGET hpp-fcl::hpp-fcl PROPERTY IMPORTED_LOCATION "${_loc}") + target_compile_definitions(hpp-fcl::hpp-fcl INTERFACE COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) +endif() diff --git a/include/coal/BV/AABB.h b/include/coal/BV/AABB.h new file mode 100644 index 000000000..8d46a5323 --- /dev/null +++ b/include/coal/BV/AABB.h @@ -0,0 +1,266 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_AABB_H +#define COAL_AABB_H + +#include "coal/data_types.h" + +namespace coal { + +struct CollisionRequest; +class Plane; +class Halfspace; + +/// @defgroup Bounding_Volume Bounding volumes +/// Classes of different types of bounding volume. +/// @{ + +/// @brief A class describing the AABB collision structure, which is a box in 3D +/// space determined by two diagonal points +class COAL_DLLAPI AABB { + public: + /// @brief The min point in the AABB + Vec3s min_; + /// @brief The max point in the AABB + Vec3s max_; + + /// @brief Creating an AABB with zero size (low bound +inf, upper bound -inf) + AABB(); + + /// @brief Creating an AABB at position v with zero size + AABB(const Vec3s& v) : min_(v), max_(v) {} + + /// @brief Creating an AABB with two endpoints a and b + AABB(const Vec3s& a, const Vec3s& b) + : min_(a.cwiseMin(b)), max_(a.cwiseMax(b)) {} + + /// @brief Creating an AABB centered as core and is of half-dimension delta + AABB(const AABB& core, const Vec3s& delta) + : min_(core.min_ - delta), max_(core.max_ + delta) {} + + /// @brief Creating an AABB contains three points + AABB(const Vec3s& a, const Vec3s& b, const Vec3s& c) + : min_(a.cwiseMin(b).cwiseMin(c)), max_(a.cwiseMax(b).cwiseMax(c)) {} + + AABB(const AABB& other) = default; + + AABB& operator=(const AABB& other) = default; + + AABB& update(const Vec3s& a, const Vec3s& b) { + min_ = a.cwiseMin(b); + max_ = a.cwiseMax(b); + return *this; + } + + /// @brief Comparison operator + bool operator==(const AABB& other) const { + return min_ == other.min_ && max_ == other.max_; + } + + bool operator!=(const AABB& other) const { return !(*this == other); } + + /// @name Bounding volume API + /// Common API to BVs. + /// @{ + + /// @brief Check whether the AABB contains a point + inline bool contain(const Vec3s& p) const { + if (p[0] < min_[0] || p[0] > max_[0]) return false; + if (p[1] < min_[1] || p[1] > max_[1]) return false; + if (p[2] < min_[2] || p[2] > max_[2]) return false; + + return true; + } + + /// @brief Check whether two AABB are overlap + inline bool overlap(const AABB& other) const { + if (min_[0] > other.max_[0]) return false; + if (min_[1] > other.max_[1]) return false; + if (min_[2] > other.max_[2]) return false; + + if (max_[0] < other.min_[0]) return false; + if (max_[1] < other.min_[1]) return false; + if (max_[2] < other.min_[2]) return false; + + return true; + } + + /// @brief Check whether AABB overlaps a plane + bool overlap(const Plane& p) const; + + /// @brief Check whether AABB overlaps a halfspace + bool overlap(const Halfspace& hs) const; + + /// @brief Check whether two AABB are overlap + bool overlap(const AABB& other, const CollisionRequest& request, + CoalScalar& sqrDistLowerBound) const; + + /// @brief Distance between two AABBs + CoalScalar distance(const AABB& other) const; + + /// @brief Distance between two AABBs; P and Q, should not be NULL, return the + /// nearest points + CoalScalar distance(const AABB& other, Vec3s* P, Vec3s* Q) const; + + /// @brief Merge the AABB and a point + inline AABB& operator+=(const Vec3s& p) { + min_ = min_.cwiseMin(p); + max_ = max_.cwiseMax(p); + return *this; + } + + /// @brief Merge the AABB and another AABB + inline AABB& operator+=(const AABB& other) { + min_ = min_.cwiseMin(other.min_); + max_ = max_.cwiseMax(other.max_); + return *this; + } + + /// @brief Return the merged AABB of current AABB and the other one + inline AABB operator+(const AABB& other) const { + AABB res(*this); + return res += other; + } + + /// @brief Size of the AABB (used in BV_Splitter to order two AABBs) + inline CoalScalar size() const { return (max_ - min_).squaredNorm(); } + + /// @brief Center of the AABB + inline Vec3s center() const { return (min_ + max_) * 0.5; } + + /// @brief Width of the AABB + inline CoalScalar width() const { return max_[0] - min_[0]; } + + /// @brief Height of the AABB + inline CoalScalar height() const { return max_[1] - min_[1]; } + + /// @brief Depth of the AABB + inline CoalScalar depth() const { return max_[2] - min_[2]; } + + /// @brief Volume of the AABB + inline CoalScalar volume() const { return width() * height() * depth(); } + + /// @} + + /// @brief Check whether the AABB contains another AABB + inline bool contain(const AABB& other) const { + return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && + (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && + (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]); + } + + /// @brief Check whether two AABB are overlap and return the overlap part + inline bool overlap(const AABB& other, AABB& overlap_part) const { + if (!overlap(other)) { + return false; + } + + overlap_part.min_ = min_.cwiseMax(other.min_); + overlap_part.max_ = max_.cwiseMin(other.max_); + return true; + } + + /// @brief Check whether two AABB are overlapped along specific axis + inline bool axisOverlap(const AABB& other, int axis_id) const { + if (min_[axis_id] > other.max_[axis_id]) return false; + if (max_[axis_id] < other.min_[axis_id]) return false; + + return true; + } + + /// @brief expand the half size of the AABB by delta, and keep the center + /// unchanged. + inline AABB& expand(const Vec3s& delta) { + min_ -= delta; + max_ += delta; + return *this; + } + + /// @brief expand the half size of the AABB by a scalar delta, and keep the + /// center unchanged. + inline AABB& expand(const CoalScalar delta) { + min_.array() -= delta; + max_.array() += delta; + return *this; + } + + /// @brief expand the aabb by increase the thickness of the plate by a ratio + inline AABB& expand(const AABB& core, CoalScalar ratio) { + min_ = min_ * ratio - core.min_; + max_ = max_ * ratio - core.max_; + return *this; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// @brief translate the center of AABB by t +static inline AABB translate(const AABB& aabb, const Vec3s& t) { + AABB res(aabb); + res.min_ += t; + res.max_ += t; + return res; +} + +static inline AABB rotate(const AABB& aabb, const Matrix3s& R) { + AABB res(R * aabb.min_); + Vec3s corner(aabb.min_); + const Eigen::DenseIndex bit[3] = {1, 2, 4}; + for (Eigen::DenseIndex ic = 1; ic < 8; + ++ic) { // ic = 0 corresponds to aabb.min_. Skip it. + for (Eigen::DenseIndex i = 0; i < 3; ++i) { + corner[i] = (ic & bit[i]) ? aabb.max_[i] : aabb.min_[i]; + } + res += R * corner; + } + return res; +} + +/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) +/// and b2 is in identity. +COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1, + const AABB& b2); + +/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) +/// and b2 is in identity. +COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1, + const AABB& b2, const CollisionRequest& request, + CoalScalar& sqrDistLowerBound); +} // namespace coal + +#endif diff --git a/include/coal/BV/BV.h b/include/coal/BV/BV.h new file mode 100644 index 000000000..2224f7918 --- /dev/null +++ b/include/coal/BV/BV.h @@ -0,0 +1,289 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_BV_H +#define COAL_BV_H + +#include "coal/BV/kDOP.h" +#include "coal/BV/AABB.h" +#include "coal/BV/OBB.h" +#include "coal/BV/RSS.h" +#include "coal/BV/OBBRSS.h" +#include "coal/BV/kIOS.h" +#include "coal/math/transform.h" + +/** @brief Main namespace */ +namespace coal { + +/// @cond IGNORE +namespace details { + +/// @brief Convert a bounding volume of type BV1 in configuration tf1 to a +/// bounding volume of type BV2 in I configuration. +template +struct Converter { + static void convert(const BV1& bv1, const Transform3s& tf1, BV2& bv2); + static void convert(const BV1& bv1, BV2& bv2); +}; + +/// @brief Convert from AABB to AABB, not very tight but is fast. +template <> +struct Converter { + static void convert(const AABB& bv1, const Transform3s& tf1, AABB& bv2) { + const Vec3s& center = bv1.center(); + CoalScalar r = (bv1.max_ - bv1.min_).norm() * 0.5; + const Vec3s center2 = tf1.transform(center); + bv2.min_ = center2 - Vec3s::Constant(r); + bv2.max_ = center2 + Vec3s::Constant(r); + } + + static void convert(const AABB& bv1, AABB& bv2) { bv2 = bv1; } +}; + +template <> +struct Converter { + static void convert(const AABB& bv1, const Transform3s& tf1, OBB& bv2) { + bv2.To = tf1.transform(bv1.center()); + bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5; + bv2.axes = tf1.getRotation(); + } + + static void convert(const AABB& bv1, OBB& bv2) { + bv2.To = bv1.center(); + bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5; + bv2.axes.setIdentity(); + } +}; + +template <> +struct Converter { + static void convert(const OBB& bv1, const Transform3s& tf1, OBB& bv2) { + bv2.extent = bv1.extent; + bv2.To = tf1.transform(bv1.To); + bv2.axes.noalias() = tf1.getRotation() * bv1.axes; + } + + static void convert(const OBB& bv1, OBB& bv2) { bv2 = bv1; } +}; + +template <> +struct Converter { + static void convert(const OBBRSS& bv1, const Transform3s& tf1, OBB& bv2) { + Converter::convert(bv1.obb, tf1, bv2); + } + + static void convert(const OBBRSS& bv1, OBB& bv2) { + Converter::convert(bv1.obb, bv2); + } +}; + +template <> +struct Converter { + static void convert(const RSS& bv1, const Transform3s& tf1, OBB& bv2) { + bv2.extent = Vec3s(bv1.length[0] * 0.5 + bv1.radius, + bv1.length[1] * 0.5 + bv1.radius, bv1.radius); + bv2.To = tf1.transform(bv1.Tr); + bv2.axes.noalias() = tf1.getRotation() * bv1.axes; + } + + static void convert(const RSS& bv1, OBB& bv2) { + bv2.extent = Vec3s(bv1.length[0] * 0.5 + bv1.radius, + bv1.length[1] * 0.5 + bv1.radius, bv1.radius); + bv2.To = bv1.Tr; + bv2.axes = bv1.axes; + } +}; + +template +struct Converter { + static void convert(const BV1& bv1, const Transform3s& tf1, AABB& bv2) { + const Vec3s& center = bv1.center(); + CoalScalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; + const Vec3s center2 = tf1.transform(center); + bv2.min_ = center2 - Vec3s::Constant(r); + bv2.max_ = center2 + Vec3s::Constant(r); + } + + static void convert(const BV1& bv1, AABB& bv2) { + const Vec3s& center = bv1.center(); + CoalScalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; + bv2.min_ = center - Vec3s::Constant(r); + bv2.max_ = center + Vec3s::Constant(r); + } +}; + +template +struct Converter { + static void convert(const BV1& bv1, const Transform3s& tf1, OBB& bv2) { + AABB bv; + Converter::convert(bv1, bv); + Converter::convert(bv, tf1, bv2); + } + + static void convert(const BV1& bv1, OBB& bv2) { + AABB bv; + Converter::convert(bv1, bv); + Converter::convert(bv, bv2); + } +}; + +template <> +struct Converter { + static void convert(const OBB& bv1, const Transform3s& tf1, RSS& bv2) { + bv2.Tr = tf1.transform(bv1.To); + bv2.axes.noalias() = tf1.getRotation() * bv1.axes; + + bv2.radius = bv1.extent[2]; + bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius); + bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius); + } + + static void convert(const OBB& bv1, RSS& bv2) { + bv2.Tr = bv1.To; + bv2.axes = bv1.axes; + + bv2.radius = bv1.extent[2]; + bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius); + bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius); + } +}; + +template <> +struct Converter { + static void convert(const RSS& bv1, const Transform3s& tf1, RSS& bv2) { + bv2.Tr = tf1.transform(bv1.Tr); + bv2.axes.noalias() = tf1.getRotation() * bv1.axes; + + bv2.radius = bv1.radius; + bv2.length[0] = bv1.length[0]; + bv2.length[1] = bv1.length[1]; + } + + static void convert(const RSS& bv1, RSS& bv2) { bv2 = bv1; } +}; + +template <> +struct Converter { + static void convert(const OBBRSS& bv1, const Transform3s& tf1, RSS& bv2) { + Converter::convert(bv1.rss, tf1, bv2); + } + + static void convert(const OBBRSS& bv1, RSS& bv2) { + Converter::convert(bv1.rss, bv2); + } +}; + +template <> +struct Converter { + static void convert(const AABB& bv1, const Transform3s& tf1, RSS& bv2) { + bv2.Tr = tf1.transform(bv1.center()); + + /// Sort the AABB edges so that AABB extents are ordered. + CoalScalar d[3] = {bv1.width(), bv1.height(), bv1.depth()}; + Eigen::DenseIndex id[3] = {0, 1, 2}; + + for (Eigen::DenseIndex i = 1; i < 3; ++i) { + for (Eigen::DenseIndex j = i; j > 0; --j) { + if (d[j] > d[j - 1]) { + { + CoalScalar tmp = d[j]; + d[j] = d[j - 1]; + d[j - 1] = tmp; + } + { + Eigen::DenseIndex tmp = id[j]; + id[j] = id[j - 1]; + id[j - 1] = tmp; + } + } + } + } + + const Vec3s extent = (bv1.max_ - bv1.min_) * 0.5; + bv2.radius = extent[id[2]]; + bv2.length[0] = (extent[id[0]] - bv2.radius) * 2; + bv2.length[1] = (extent[id[1]] - bv2.radius) * 2; + + const Matrix3s& R = tf1.getRotation(); + const bool left_hand = (id[0] == (id[1] + 1) % 3); + if (left_hand) + bv2.axes.col(0) = -R.col(id[0]); + else + bv2.axes.col(0) = R.col(id[0]); + bv2.axes.col(1) = R.col(id[1]); + bv2.axes.col(2) = R.col(id[2]); + } + + static void convert(const AABB& bv1, RSS& bv2) { + convert(bv1, Transform3s(), bv2); + } +}; + +template <> +struct Converter { + static void convert(const AABB& bv1, const Transform3s& tf1, OBBRSS& bv2) { + Converter::convert(bv1, tf1, bv2.obb); + Converter::convert(bv1, tf1, bv2.rss); + } + + static void convert(const AABB& bv1, OBBRSS& bv2) { + Converter::convert(bv1, bv2.obb); + Converter::convert(bv1, bv2.rss); + } +}; + +} // namespace details + +/// @endcond + +/// @brief Convert a bounding volume of type BV1 in configuration tf1 to +/// bounding volume of type BV2 in identity configuration. +template +static inline void convertBV(const BV1& bv1, const Transform3s& tf1, BV2& bv2) { + details::Converter::convert(bv1, tf1, bv2); +} + +/// @brief Convert a bounding volume of type BV1 to bounding volume of type BV2 +/// in identity configuration. +template +static inline void convertBV(const BV1& bv1, BV2& bv2) { + details::Converter::convert(bv1, bv2); +} + +} // namespace coal + +#endif diff --git a/include/coal/BV/BV_node.h b/include/coal/BV/BV_node.h new file mode 100644 index 000000000..b858308c5 --- /dev/null +++ b/include/coal/BV/BV_node.h @@ -0,0 +1,166 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_BV_NODE_H +#define COAL_BV_NODE_H + +#include "coal/data_types.h" +#include "coal/BV/BV.h" + +namespace coal { + +/// @defgroup Construction_Of_BVH Construction of BVHModel +/// Classes which are used to build a BVHModel (Bounding Volume Hierarchy) +/// @{ + +/// @brief BVNodeBase encodes the tree structure for BVH +struct COAL_DLLAPI BVNodeBase { + /// @brief An index for first child node or primitive + /// If the value is positive, it is the index of the first child bv node + /// If the value is negative, it is -(primitive index + 1) + /// Zero is not used. + int first_child; + + /// @brief The start id the primitive belonging to the current node. The index + /// is referred to the primitive_indices in BVHModel and from that we can + /// obtain the primitive's index in original data indirectly. + unsigned int first_primitive; + + /// @brief The number of primitives belonging to the current node + unsigned int num_primitives; + + /// @brief Default constructor + BVNodeBase() + : first_child(0), + first_primitive( + (std::numeric_limits::max)()) // value we should help + // to raise an issue + , + num_primitives(0) {} + + /// @brief Equality operator + bool operator==(const BVNodeBase& other) const { + return first_child == other.first_child && + first_primitive == other.first_primitive && + num_primitives == other.num_primitives; + } + + /// @brief Difference operator + bool operator!=(const BVNodeBase& other) const { return !(*this == other); } + + /// @brief Whether current node is a leaf node (i.e. contains a primitive + /// index + inline bool isLeaf() const { return first_child < 0; } + + /// @brief Return the primitive index. The index is referred to the original + /// data (i.e. vertices or tri_indices) in BVHModel + inline int primitiveId() const { return -(first_child + 1); } + + /// @brief Return the index of the first child. The index is referred to the + /// bounding volume array (i.e. bvs) in BVHModel + inline int leftChild() const { return first_child; } + + /// @brief Return the index of the second child. The index is referred to the + /// bounding volume array (i.e. bvs) in BVHModel + inline int rightChild() const { return first_child + 1; } +}; + +/// @brief A class describing a bounding volume node. It includes the tree +/// structure providing in BVNodeBase and also the geometry data provided in BV +/// template parameter. +template +struct COAL_DLLAPI BVNode : public BVNodeBase { + typedef BVNodeBase Base; + + /// @brief bounding volume storing the geometry + BV bv; + + /// @brief Equality operator + bool operator==(const BVNode& other) const { + return Base::operator==(other) && bv == other.bv; + } + + /// @brief Difference operator + bool operator!=(const BVNode& other) const { return !(*this == other); } + + /// @brief Check whether two BVNode collide + bool overlap(const BVNode& other) const { return bv.overlap(other.bv); } + /// @brief Check whether two BVNode collide + bool overlap(const BVNode& other, const CollisionRequest& request, + CoalScalar& sqrDistLowerBound) const { + return bv.overlap(other.bv, request, sqrDistLowerBound); + } + + /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and + /// the underlying BV supports distance, return the nearest points. + CoalScalar distance(const BVNode& other, Vec3s* P1 = NULL, + Vec3s* P2 = NULL) const { + return bv.distance(other.bv, P1, P2); + } + + /// @brief Access to the center of the BV + Vec3s getCenter() const { return bv.center(); } + + /// @brief Access to the orientation of the BV + const Matrix3s& getOrientation() const { + static const Matrix3s id3 = Matrix3s::Identity(); + return id3; + } + + /// \cond + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// \endcond +}; + +template <> +inline const Matrix3s& BVNode::getOrientation() const { + return bv.axes; +} + +template <> +inline const Matrix3s& BVNode::getOrientation() const { + return bv.axes; +} + +template <> +inline const Matrix3s& BVNode::getOrientation() const { + return bv.obb.axes; +} + +} // namespace coal + +#endif diff --git a/include/coal/BV/OBB.h b/include/coal/BV/OBB.h new file mode 100644 index 000000000..8255f023b --- /dev/null +++ b/include/coal/BV/OBB.h @@ -0,0 +1,150 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_OBB_H +#define COAL_OBB_H + +#include "coal/data_types.h" + +namespace coal { + +struct CollisionRequest; + +/// @addtogroup Bounding_Volume +/// @{ + +/// @brief Oriented bounding box class +struct COAL_DLLAPI OBB { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// @brief Orientation of OBB. axis[i] is the ith column of the orientation + /// matrix for the box; it is also the i-th principle direction of the box. We + /// assume that axis[0] corresponds to the axis with the longest box edge, + /// axis[1] corresponds to the shorter one and axis[2] corresponds to the + /// shortest one. + Matrix3s axes; + + /// @brief Center of OBB + Vec3s To; + + /// @brief Half dimensions of OBB + Vec3s extent; + + OBB() : axes(Matrix3s::Zero()), To(Vec3s::Zero()), extent(Vec3s::Zero()) {} + + /// @brief Equality operator + bool operator==(const OBB& other) const { + return axes == other.axes && To == other.To && extent == other.extent; + } + + /// @brief Difference operator + bool operator!=(const OBB& other) const { return !(*this == other); } + + /// @brief Check whether the OBB contains a point. + bool contain(const Vec3s& p) const; + + /// Check collision between two OBB + /// @return true if collision happens. + bool overlap(const OBB& other) const; + + /// Check collision between two OBB + /// @return true if collision happens. + /// @retval sqrDistLowerBound squared lower bound on distance between boxes if + /// they do not overlap. + bool overlap(const OBB& other, const CollisionRequest& request, + CoalScalar& sqrDistLowerBound) const; + + /// @brief Distance between two OBBs, not implemented. + CoalScalar distance(const OBB& other, Vec3s* P = NULL, Vec3s* Q = NULL) const; + + /// @brief A simple way to merge the OBB and a point (the result is not + /// compact). + OBB& operator+=(const Vec3s& p); + + /// @brief Merge the OBB and another OBB (the result is not compact). + OBB& operator+=(const OBB& other) { + *this = *this + other; + return *this; + } + + /// @brief Return the merged OBB of current OBB and the other one (the result + /// is not compact). + OBB operator+(const OBB& other) const; + + /// @brief Size of the OBB (used in BV_Splitter to order two OBBs) + inline CoalScalar size() const { return extent.squaredNorm(); } + + /// @brief Center of the OBB + inline const Vec3s& center() const { return To; } + + /// @brief Width of the OBB. + inline CoalScalar width() const { return 2 * extent[0]; } + + /// @brief Height of the OBB. + inline CoalScalar height() const { return 2 * extent[1]; } + + /// @brief Depth of the OBB + inline CoalScalar depth() const { return 2 * extent[2]; } + + /// @brief Volume of the OBB + inline CoalScalar volume() const { return width() * height() * depth(); } +}; + +/// @brief Translate the OBB bv +COAL_DLLAPI OBB translate(const OBB& bv, const Vec3s& t); + +/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and +/// b2 is in identity. +COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, + const OBB& b2); + +/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and +/// b2 is in identity. +COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, + const OBB& b2, const CollisionRequest& request, + CoalScalar& sqrDistLowerBound); + +/// Check collision between two boxes +/// @param B, T orientation and position of first box, +/// @param a half dimensions of first box, +/// @param b half dimensions of second box. +/// The second box is in identity configuration. +COAL_DLLAPI bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b); +} // namespace coal + +#endif diff --git a/include/coal/BV/OBBRSS.h b/include/coal/BV/OBBRSS.h new file mode 100644 index 000000000..97ea6bb10 --- /dev/null +++ b/include/coal/BV/OBBRSS.h @@ -0,0 +1,160 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_OBBRSS_H +#define COAL_OBBRSS_H + +#include "coal/BV/OBB.h" +#include "coal/BV/RSS.h" + +namespace coal { + +struct CollisionRequest; + +/// @addtogroup Bounding_Volume +/// @{ + +/// @brief Class merging the OBB and RSS, can handle collision and distance +/// simultaneously +struct COAL_DLLAPI OBBRSS { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// @brief OBB member, for rotation + OBB obb; + + /// @brief RSS member, for distance + RSS rss; + + /// @brief Equality operator + bool operator==(const OBBRSS& other) const { + return obb == other.obb && rss == other.rss; + } + + /// @brief Difference operator + bool operator!=(const OBBRSS& other) const { return !(*this == other); } + + /// @brief Check whether the OBBRSS contains a point + inline bool contain(const Vec3s& p) const { return obb.contain(p); } + + /// @brief Check collision between two OBBRSS + bool overlap(const OBBRSS& other) const { return obb.overlap(other.obb); } + + /// Check collision between two OBBRSS + /// @retval sqrDistLowerBound squared lower bound on distance between + /// objects if they do not overlap. + bool overlap(const OBBRSS& other, const CollisionRequest& request, + CoalScalar& sqrDistLowerBound) const { + return obb.overlap(other.obb, request, sqrDistLowerBound); + } + + /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the + /// nearest points + CoalScalar distance(const OBBRSS& other, Vec3s* P = NULL, + Vec3s* Q = NULL) const { + return rss.distance(other.rss, P, Q); + } + + /// @brief Merge the OBBRSS and a point + OBBRSS& operator+=(const Vec3s& p) { + obb += p; + rss += p; + return *this; + } + + /// @brief Merge two OBBRSS + OBBRSS& operator+=(const OBBRSS& other) { + *this = *this + other; + return *this; + } + + /// @brief Merge two OBBRSS + OBBRSS operator+(const OBBRSS& other) const { + OBBRSS result; + result.obb = obb + other.obb; + result.rss = rss + other.rss; + return result; + } + + /// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS) + inline CoalScalar size() const { return obb.size(); } + + /// @brief Center of the OBBRSS + inline const Vec3s& center() const { return obb.center(); } + + /// @brief Width of the OBRSS + inline CoalScalar width() const { return obb.width(); } + + /// @brief Height of the OBBRSS + inline CoalScalar height() const { return obb.height(); } + + /// @brief Depth of the OBBRSS + inline CoalScalar depth() const { return obb.depth(); } + + /// @brief Volume of the OBBRSS + inline CoalScalar volume() const { return obb.volume(); } +}; + +/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) +/// and b2 is in indentity +inline bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1, + const OBBRSS& b2) { + return overlap(R0, T0, b1.obb, b2.obb); +} + +/// Check collision between two OBBRSS +/// @param R0, T0 configuration of b1 +/// @param b1 first OBBRSS in configuration (R0, T0) +/// @param b2 second OBBRSS in identity position +/// @retval sqrDistLowerBound squared lower bound on the distance if OBBRSS do +/// not overlap. +inline bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1, + const OBBRSS& b2, const CollisionRequest& request, + CoalScalar& sqrDistLowerBound) { + return overlap(R0, T0, b1.obb, b2.obb, request, sqrDistLowerBound); +} + +/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) +/// and b2 is in indentity; P and Q, is not NULL, returns the nearest points +inline CoalScalar distance(const Matrix3s& R0, const Vec3s& T0, + const OBBRSS& b1, const OBBRSS& b2, Vec3s* P = NULL, + Vec3s* Q = NULL) { + return distance(R0, T0, b1.rss, b2.rss, P, Q); +} + +} // namespace coal + +#endif diff --git a/include/coal/BV/RSS.h b/include/coal/BV/RSS.h new file mode 100644 index 000000000..8bf2bd07a --- /dev/null +++ b/include/coal/BV/RSS.h @@ -0,0 +1,173 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_RSS_H +#define COAL_RSS_H + +#include "coal/data_types.h" + +#include + +namespace coal { + +struct CollisionRequest; + +/// @addtogroup Bounding_Volume +/// @{ + +/// @brief A class for rectangle sphere-swept bounding volume +struct COAL_DLLAPI RSS { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// @brief Orientation of RSS. axis[i] is the ith column of the orientation + /// matrix for the RSS; it is also the i-th principle direction of the RSS. We + /// assume that axis[0] corresponds to the axis with the longest length, + /// axis[1] corresponds to the shorter one and axis[2] corresponds to the + /// shortest one. + Matrix3s axes; + + /// @brief Origin of the rectangle in RSS + Vec3s Tr; + + /// @brief Side lengths of rectangle + CoalScalar length[2]; + + /// @brief Radius of sphere summed with rectangle to form RSS + CoalScalar radius; + + ///  @brief Default constructor with default values + RSS() : axes(Matrix3s::Zero()), Tr(Vec3s::Zero()), radius(-1) { + length[0] = 0; + length[1] = 0; + } + + /// @brief Equality operator + bool operator==(const RSS& other) const { + return axes == other.axes && Tr == other.Tr && + length[0] == other.length[0] && length[1] == other.length[1] && + radius == other.radius; + } + + /// @brief Difference operator + bool operator!=(const RSS& other) const { return !(*this == other); } + + /// @brief Check whether the RSS contains a point + bool contain(const Vec3s& p) const; + + /// @brief Check collision between two RSS + bool overlap(const RSS& other) const; + + /// Not implemented + bool overlap(const RSS& other, const CollisionRequest&, + CoalScalar& sqrDistLowerBound) const { + sqrDistLowerBound = sqrt(-1); + return overlap(other); + } + + /// @brief the distance between two RSS; P and Q, if not NULL, return the + /// nearest points + CoalScalar distance(const RSS& other, Vec3s* P = NULL, Vec3s* Q = NULL) const; + + /// @brief A simple way to merge the RSS and a point, not compact. + /// @todo This function may have some bug. + RSS& operator+=(const Vec3s& p); + + /// @brief Merge the RSS and another RSS + inline RSS& operator+=(const RSS& other) { + *this = *this + other; + return *this; + } + + /// @brief Return the merged RSS of current RSS and the other one + RSS operator+(const RSS& other) const; + + /// @brief Size of the RSS (used in BV_Splitter to order two RSSs) + inline CoalScalar size() const { + return (std::sqrt(length[0] * length[0] + length[1] * length[1]) + + 2 * radius); + } + + /// @brief The RSS center + inline const Vec3s& center() const { return Tr; } + + /// @brief Width of the RSS + inline CoalScalar width() const { return length[0] + 2 * radius; } + + /// @brief Height of the RSS + inline CoalScalar height() const { return length[1] + 2 * radius; } + + /// @brief Depth of the RSS + inline CoalScalar depth() const { return 2 * radius; } + + /// @brief Volume of the RSS + inline CoalScalar volume() const { + return (length[0] * length[1] * 2 * radius + + 4 * boost::math::constants::pi() * radius * radius * + radius); + } + + /// @brief Check collision between two RSS and return the overlap part. + /// For RSS, we return nothing, as the overlap part of two RSSs usually is not + /// a RSS. + bool overlap(const RSS& other, RSS& /*overlap_part*/) const { + return overlap(other); + } +}; + +/// @brief distance between two RSS bounding volumes +/// P and Q (optional return values) are the closest points in the rectangles, +/// not the RSS. But the direction P - Q is the correct direction for cloest +/// points Notice that P and Q are both in the local frame of the first RSS (not +/// global frame and not even the local frame of object 1) +COAL_DLLAPI CoalScalar distance(const Matrix3s& R0, const Vec3s& T0, + const RSS& b1, const RSS& b2, Vec3s* P = NULL, + Vec3s* Q = NULL); + +/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and +/// b2 is in identity. +COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, + const RSS& b2); + +/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and +/// b2 is in identity. +COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, + const RSS& b2, const CollisionRequest& request, + CoalScalar& sqrDistLowerBound); + +} // namespace coal + +#endif diff --git a/include/coal/BV/kDOP.h b/include/coal/BV/kDOP.h new file mode 100644 index 000000000..cb8377526 --- /dev/null +++ b/include/coal/BV/kDOP.h @@ -0,0 +1,190 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_KDOP_H +#define COAL_KDOP_H + +#include "coal/fwd.hh" +#include "coal/data_types.h" + +namespace coal { + +struct CollisionRequest; + +/// @addtogroup Bounding_Volume +/// @{ + +/// @brief KDOP class describes the KDOP collision structures. K is set as the +/// template parameter, which should be 16, 18, or 24 +/// The KDOP structure is defined by some pairs of parallel planes defined by +/// some axes. +/// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off +/// some space of the edges: +/// (-1,0,0) and (1,0,0) -> indices 0 and 8 +/// (0,-1,0) and (0,1,0) -> indices 1 and 9 +/// (0,0,-1) and (0,0,1) -> indices 2 and 10 +/// (-1,-1,0) and (1,1,0) -> indices 3 and 11 +/// (-1,0,-1) and (1,0,1) -> indices 4 and 12 +/// (0,-1,-1) and (0,1,1) -> indices 5 and 13 +/// (-1,1,0) and (1,-1,0) -> indices 6 and 14 +/// (-1,0,1) and (1,0,-1) -> indices 7 and 15 +/// For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off +/// some space of the edges: +/// (-1,0,0) and (1,0,0) -> indices 0 and 9 +/// (0,-1,0) and (0,1,0) -> indices 1 and 10 +/// (0,0,-1) and (0,0,1) -> indices 2 and 11 +/// (-1,-1,0) and (1,1,0) -> indices 3 and 12 +/// (-1,0,-1) and (1,0,1) -> indices 4 and 13 +/// (0,-1,-1) and (0,1,1) -> indices 5 and 14 +/// (-1,1,0) and (1,-1,0) -> indices 6 and 15 +/// (-1,0,1) and (1,0,-1) -> indices 7 and 16 +/// (0,-1,1) and (0,1,-1) -> indices 8 and 17 +/// For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off +/// some space of the edges: +/// (-1,0,0) and (1,0,0) -> indices 0 and 12 +/// (0,-1,0) and (0,1,0) -> indices 1 and 13 +/// (0,0,-1) and (0,0,1) -> indices 2 and 14 +/// (-1,-1,0) and (1,1,0) -> indices 3 and 15 +/// (-1,0,-1) and (1,0,1) -> indices 4 and 16 +/// (0,-1,-1) and (0,1,1) -> indices 5 and 17 +/// (-1,1,0) and (1,-1,0) -> indices 6 and 18 +/// (-1,0,1) and (1,0,-1) -> indices 7 and 19 +/// (0,-1,1) and (0,1,-1) -> indices 8 and 20 +/// (-1, -1, 1) and (1, 1, -1) --> indices 9 and 21 +/// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22 +/// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23 +template +class COAL_DLLAPI KDOP { + protected: + /// @brief Origin's distances to N KDOP planes + Eigen::Array dist_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// @brief Creating kDOP containing nothing + KDOP(); + + /// @brief Creating kDOP containing only one point + KDOP(const Vec3s& v); + + /// @brief Creating kDOP containing two points + KDOP(const Vec3s& a, const Vec3s& b); + + /// @brief Equality operator + bool operator==(const KDOP& other) const { + return (dist_ == other.dist_).all(); + } + + /// @brief Difference operator + bool operator!=(const KDOP& other) const { + return (dist_ != other.dist_).any(); + } + + /// @brief Check whether two KDOPs overlap. + bool overlap(const KDOP& other) const; + + /// @brief Check whether two KDOPs overlap. + /// @return true if collision happens. + /// @retval sqrDistLowerBound squared lower bound on distance between boxes if + /// they do not overlap. + bool overlap(const KDOP& other, const CollisionRequest& request, + CoalScalar& sqrDistLowerBound) const; + + /// @brief The distance between two KDOP. Not implemented. + CoalScalar distance(const KDOP& other, Vec3s* P = NULL, + Vec3s* Q = NULL) const; + + /// @brief Merge the point and the KDOP + KDOP& operator+=(const Vec3s& p); + + /// @brief Merge two KDOPs + KDOP& operator+=(const KDOP& other); + + /// @brief Create a KDOP by mergin two KDOPs + KDOP operator+(const KDOP& other) const; + + /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs) + inline CoalScalar size() const { + return width() * width() + height() * height() + depth() * depth(); + } + + /// @brief The (AABB) center + inline Vec3s center() const { + return (dist_.template head<3>() + dist_.template segment<3>(N / 2)) / 2; + } + + /// @brief The (AABB) width + inline CoalScalar width() const { return dist_[N / 2] - dist_[0]; } + + /// @brief The (AABB) height + inline CoalScalar height() const { return dist_[N / 2 + 1] - dist_[1]; } + + /// @brief The (AABB) depth + inline CoalScalar depth() const { return dist_[N / 2 + 2] - dist_[2]; } + + /// @brief The (AABB) volume + inline CoalScalar volume() const { return width() * height() * depth(); } + + inline CoalScalar dist(short i) const { return dist_[i]; } + + inline CoalScalar& dist(short i) { return dist_[i]; } + + //// @brief Check whether one point is inside the KDOP + bool inside(const Vec3s& p) const; +}; + +template +bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP& /*b1*/, + const KDOP& /*b2*/) { + COAL_THROW_PRETTY("not implemented", std::logic_error); +} + +template +bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP& /*b1*/, + const KDOP& /*b2*/, const CollisionRequest& /*request*/, + CoalScalar& /*sqrDistLowerBound*/) { + COAL_THROW_PRETTY("not implemented", std::logic_error); +} + +/// @brief translate the KDOP BV +template +COAL_DLLAPI KDOP translate(const KDOP& bv, const Vec3s& t); + +} // namespace coal + +#endif diff --git a/include/coal/BV/kIOS.h b/include/coal/BV/kIOS.h new file mode 100644 index 000000000..83277ab6a --- /dev/null +++ b/include/coal/BV/kIOS.h @@ -0,0 +1,193 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_KIOS_H +#define COAL_KIOS_H + +#include "coal/BV/OBB.h" + +namespace coal { + +struct CollisionRequest; + +/// @addtogroup Bounding_Volume +/// @{ + +/// @brief A class describing the kIOS collision structure, which is a set of +/// spheres. +class COAL_DLLAPI kIOS { + /// @brief One sphere in kIOS + struct COAL_DLLAPI kIOS_Sphere { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Vec3s o; + CoalScalar r; + + bool operator==(const kIOS_Sphere& other) const { + return o == other.o && r == other.r; + } + + bool operator!=(const kIOS_Sphere& other) const { + return !(*this == other); + } + }; + + /// @brief generate one sphere enclosing two spheres + static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, + const kIOS_Sphere& s1) { + Vec3s d = s1.o - s0.o; + CoalScalar dist2 = d.squaredNorm(); + CoalScalar diff_r = s1.r - s0.r; + + /** The sphere with the larger radius encloses the other */ + if (diff_r * diff_r >= dist2) { + if (s1.r > s0.r) + return s1; + else + return s0; + } else /** spheres partially overlapping or disjoint */ + { + float dist = (float)std::sqrt(dist2); + kIOS_Sphere s; + s.r = dist + s0.r + s1.r; + if (dist > 0) + s.o = s0.o + d * ((s.r - s0.r) / dist); + else + s.o = s0.o; + return s; + } + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// @brief Equality operator + bool operator==(const kIOS& other) const { + bool res = obb == other.obb && num_spheres == other.num_spheres; + if (!res) return false; + + for (size_t k = 0; k < num_spheres; ++k) { + if (spheres[k] != other.spheres[k]) return false; + } + + return true; + } + + /// @brief Difference operator + bool operator!=(const kIOS& other) const { return !(*this == other); } + + static constexpr size_t max_num_spheres = 5; + + /// @brief The (at most) five spheres for intersection + kIOS_Sphere spheres[max_num_spheres]; + + /// @brief The number of spheres, no larger than 5 + unsigned int num_spheres; + + /// @ OBB related with kIOS + OBB obb; + + /// @brief Check whether the kIOS contains a point + bool contain(const Vec3s& p) const; + + /// @brief Check collision between two kIOS + bool overlap(const kIOS& other) const; + + /// @brief Check collision between two kIOS + bool overlap(const kIOS& other, const CollisionRequest&, + CoalScalar& sqrDistLowerBound) const; + + /// @brief The distance between two kIOS + CoalScalar distance(const kIOS& other, Vec3s* P = NULL, + Vec3s* Q = NULL) const; + + /// @brief A simple way to merge the kIOS and a point + kIOS& operator+=(const Vec3s& p); + + /// @brief Merge the kIOS and another kIOS + kIOS& operator+=(const kIOS& other) { + *this = *this + other; + return *this; + } + + /// @brief Return the merged kIOS of current kIOS and the other one + kIOS operator+(const kIOS& other) const; + + /// @brief size of the kIOS (used in BV_Splitter to order two kIOSs) + CoalScalar size() const; + + /// @brief Center of the kIOS + const Vec3s& center() const { return spheres[0].o; } + + /// @brief Width of the kIOS + CoalScalar width() const; + + /// @brief Height of the kIOS + CoalScalar height() const; + + /// @brief Depth of the kIOS + CoalScalar depth() const; + + /// @brief Volume of the kIOS + CoalScalar volume() const; +}; + +/// @brief Translate the kIOS BV +COAL_DLLAPI kIOS translate(const kIOS& bv, const Vec3s& t); + +/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) +/// and b2 is in identity. +/// @todo Not efficient +COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, + const kIOS& b2); + +/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) +/// and b2 is in identity. +/// @todo Not efficient +COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, + const kIOS& b2, const CollisionRequest& request, + CoalScalar& sqrDistLowerBound); + +/// @brief Approximate distance between two kIOS bounding volumes +/// @todo P and Q is not returned, need implementation +COAL_DLLAPI CoalScalar distance(const Matrix3s& R0, const Vec3s& T0, + const kIOS& b1, const kIOS& b2, Vec3s* P = NULL, + Vec3s* Q = NULL); + +} // namespace coal + +#endif diff --git a/include/coal/BVH/BVH_front.h b/include/coal/BVH/BVH_front.h new file mode 100644 index 000000000..fe471b9b3 --- /dev/null +++ b/include/coal/BVH/BVH_front.h @@ -0,0 +1,76 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_BVH_FRONT_H +#define COAL_BVH_FRONT_H + +#include + +#include "coal/config.hh" + +namespace coal { + +/// @brief Front list acceleration for collision +/// Front list is a set of internal and leaf nodes in the BVTT hierarchy, where +/// the traversal terminates while performing a query during a given time +/// instance. The front list reflects the subset of a BVTT that is traversed for +/// that particular proximity query. +struct COAL_DLLAPI BVHFrontNode { + /// @brief The nodes to start in the future, i.e. the wave front of the + /// traversal tree. + unsigned int left, right; + + /// @brief The front node is not valid when collision is detected on the front + /// node. + bool valid; + + BVHFrontNode(unsigned int left_, unsigned int right_) + : left(left_), right(right_), valid(true) {} +}; + +/// @brief BVH front list is a list of front nodes. +typedef std::list BVHFrontList; + +/// @brief Add new front node into the front list +inline void updateFrontList(BVHFrontList* front_list, unsigned int b1, + unsigned int b2) { + if (front_list) front_list->push_back(BVHFrontNode(b1, b2)); +} + +} // namespace coal + +#endif diff --git a/include/coal/BVH/BVH_internal.h b/include/coal/BVH/BVH_internal.h new file mode 100644 index 000000000..49884639a --- /dev/null +++ b/include/coal/BVH/BVH_internal.h @@ -0,0 +1,87 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_BVH_INTERNAL_H +#define COAL_BVH_INTERNAL_H + +#include "coal/data_types.h" + +namespace coal { + +/// @brief States for BVH construction +/// empty->begun->processed ->replace_begun->processed -> ...... +/// | +/// |-> update_begun -> updated -> ..... +enum BVHBuildState { + BVH_BUILD_STATE_EMPTY, /// empty state, immediately after constructor + BVH_BUILD_STATE_BEGUN, /// after beginModel(), state for adding geometry + /// primitives + BVH_BUILD_STATE_PROCESSED, /// after tree has been build, ready for cd use + BVH_BUILD_STATE_UPDATE_BEGUN, /// after beginUpdateModel(), state for + /// updating geometry primitives + BVH_BUILD_STATE_UPDATED, /// after tree has been build for updated geometry, + /// ready for ccd use + BVH_BUILD_STATE_REPLACE_BEGUN /// after beginReplaceModel(), state for + /// replacing geometry primitives +}; + +/// @brief Error code for BVH +enum BVHReturnCode { + BVH_OK = 0, /// BVH is valid + BVH_ERR_MODEL_OUT_OF_MEMORY = + -1, /// Cannot allocate memory for vertices and triangles + BVH_ERR_BUILD_OUT_OF_SEQUENCE = + -2, /// BVH construction does not follow correct sequence + BVH_ERR_BUILD_EMPTY_MODEL = -3, /// BVH geometry is not prepared + BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME = + -4, /// BVH geometry in previous frame is not prepared + BVH_ERR_UNSUPPORTED_FUNCTION = -5, /// BVH funtion is not supported + BVH_ERR_UNUPDATED_MODEL = -6, /// BVH model update failed + BVH_ERR_INCORRECT_DATA = -7, /// BVH data is not valid + BVH_ERR_UNKNOWN = -8 /// Unknown failure +}; + +/// @brief BVH model type +enum BVHModelType { + BVH_MODEL_UNKNOWN, /// @brief unknown model type + BVH_MODEL_TRIANGLES, /// @brief triangle model + BVH_MODEL_POINTCLOUD /// @brief point cloud model +}; + +} // namespace coal + +#endif diff --git a/include/coal/BVH/BVH_model.h b/include/coal/BVH/BVH_model.h new file mode 100644 index 000000000..722578f48 --- /dev/null +++ b/include/coal/BVH/BVH_model.h @@ -0,0 +1,539 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2020-2022, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_BVH_MODEL_H +#define COAL_BVH_MODEL_H + +#include "coal/fwd.hh" +#include "coal/collision_object.h" +#include "coal/BVH/BVH_internal.h" +#include "coal/BV/BV_node.h" + +#include +#include +#include + +namespace coal { + +/// @addtogroup Construction_Of_BVH +/// @{ + +class ConvexBase; + +template +class BVFitter; +template +class BVSplitter; + +/// @brief A base class describing the bounding hierarchy of a mesh model or a +/// point cloud model (which is viewed as a degraded version of mesh) +class COAL_DLLAPI BVHModelBase : public CollisionGeometry { + public: + /// @brief Geometry point data + std::shared_ptr> vertices; + + /// @brief Geometry triangle index data, will be NULL for point clouds + std::shared_ptr> tri_indices; + + /// @brief Geometry point data in previous frame + std::shared_ptr> prev_vertices; + + /// @brief Number of triangles + unsigned int num_tris; + + /// @brief Number of points + unsigned int num_vertices; + + /// @brief The state of BVH building process + BVHBuildState build_state; + + /// @brief Convex representation of this object + shared_ptr convex; + + /// @brief Model type described by the instance + BVHModelType getModelType() const { + if (num_tris && num_vertices) + return BVH_MODEL_TRIANGLES; + else if (num_vertices) + return BVH_MODEL_POINTCLOUD; + else + return BVH_MODEL_UNKNOWN; + } + + /// @brief Constructing an empty BVH + BVHModelBase(); + + /// @brief copy from another BVH + BVHModelBase(const BVHModelBase& other); + + /// @brief deconstruction, delete mesh data related. + virtual ~BVHModelBase() {} + + /// @brief Get the object type: it is a BVH + OBJECT_TYPE getObjectType() const { return OT_BVH; } + + /// @brief Compute the AABB for the BVH, used for broad-phase collision + void computeLocalAABB(); + + /// @brief Begin a new BVH model + int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0); + + /// @brief Add one point in the new BVH model + int addVertex(const Vec3s& p); + + /// @brief Add points in the new BVH model + int addVertices(const MatrixX3s& points); + + /// @brief Add triangles in the new BVH model + int addTriangles(const Matrixx3i& triangles); + + /// @brief Add one triangle in the new BVH model + int addTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3); + + /// @brief Add a set of triangles in the new BVH model + int addSubModel(const std::vector& ps, + const std::vector& ts); + + /// @brief Add a set of points in the new BVH model + int addSubModel(const std::vector& ps); + + /// @brief End BVH model construction, will build the bounding volume + /// hierarchy + int endModel(); + + /// @brief Replace the geometry information of current frame (i.e. should have + /// the same mesh topology with the previous frame) + int beginReplaceModel(); + + /// @brief Replace one point in the old BVH model + int replaceVertex(const Vec3s& p); + + /// @brief Replace one triangle in the old BVH model + int replaceTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3); + + /// @brief Replace a set of points in the old BVH model + int replaceSubModel(const std::vector& ps); + + /// @brief End BVH model replacement, will also refit or rebuild the bounding + /// volume hierarchy + int endReplaceModel(bool refit = true, bool bottomup = true); + + /// @brief Replace the geometry information of current frame (i.e. should have + /// the same mesh topology with the previous frame). The current frame will be + /// saved as the previous frame in prev_vertices. + int beginUpdateModel(); + + /// @brief Update one point in the old BVH model + int updateVertex(const Vec3s& p); + + /// @brief Update one triangle in the old BVH model + int updateTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3); + + /// @brief Update a set of points in the old BVH model + int updateSubModel(const std::vector& ps); + + /// @brief End BVH model update, will also refit or rebuild the bounding + /// volume hierarchy + int endUpdateModel(bool refit = true, bool bottomup = true); + + /// @brief Build this \ref Convex "Convex" representation of this + /// model. The result is stored in attribute \ref convex. \note this only + /// takes the points of this model. It does not check that the + /// object is convex. It does not compute a convex hull. + void buildConvexRepresentation(bool share_memory); + + /// @brief Build a convex hull + /// and store it in attribute \ref convex. + /// \param keepTriangle whether the convex should be triangulated. + /// \param qhullCommand see \ref ConvexBase::convexHull. + /// \return \c true if this object is convex, hence the convex hull represents + /// the same object. + /// \sa ConvexBase::convexHull + /// \warning At the moment, the return value only checks whether there are as + /// many points in the convex hull as in the original object. This is + /// neither necessary (duplicated vertices get merged) nor sufficient + /// (think of a U with 4 vertices and 3 edges). + bool buildConvexHull(bool keepTriangle, const char* qhullCommand = NULL); + + virtual int memUsage(const bool msg = false) const = 0; + + /// @brief This is a special acceleration: BVH_model default stores the BV's + /// transform in world coordinate. However, we can also store each BV's + /// transform related to its parent BV node. When traversing the BVH, this can + /// save one matrix transformation. + virtual void makeParentRelative() = 0; + + Vec3s computeCOM() const { + CoalScalar vol = 0; + Vec3s com(0, 0, 0); + if (!(vertices.get())) { + std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " + "vertices." + << std::endl; + return com; + } + const std::vector& vertices_ = *vertices; + if (!(tri_indices.get())) { + std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " + "triangles." + << std::endl; + return com; + } + const std::vector& tri_indices_ = *tri_indices; + + for (unsigned int i = 0; i < num_tris; ++i) { + const Triangle& tri = tri_indices_[i]; + CoalScalar d_six_vol = + (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]); + vol += d_six_vol; + com += (vertices_[tri[0]] + vertices_[tri[1]] + vertices_[tri[2]]) * + d_six_vol; + } + + return com / (vol * 4); + } + + CoalScalar computeVolume() const { + CoalScalar vol = 0; + if (!(vertices.get())) { + std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " + "vertices." + << std::endl; + return vol; + } + const std::vector& vertices_ = *vertices; + if (!(tri_indices.get())) { + std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " + "triangles." + << std::endl; + return vol; + } + const std::vector& tri_indices_ = *tri_indices; + for (unsigned int i = 0; i < num_tris; ++i) { + const Triangle& tri = tri_indices_[i]; + CoalScalar d_six_vol = + (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]); + vol += d_six_vol; + } + + return vol / 6; + } + + Matrix3s computeMomentofInertia() const { + Matrix3s C = Matrix3s::Zero(); + + Matrix3s C_canonical; + C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0, + 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0; + + if (!(vertices.get())) { + std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does " + "not contain vertices." + << std::endl; + return C; + } + const std::vector& vertices_ = *vertices; + if (!(vertices.get())) { + std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does " + "not contain vertices." + << std::endl; + return C; + } + const std::vector& tri_indices_ = *tri_indices; + for (unsigned int i = 0; i < num_tris; ++i) { + const Triangle& tri = tri_indices_[i]; + const Vec3s& v1 = vertices_[tri[0]]; + const Vec3s& v2 = vertices_[tri[1]]; + const Vec3s& v3 = vertices_[tri[2]]; + Matrix3s A; + A << v1.transpose(), v2.transpose(), v3.transpose(); + C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3); + } + + return C.trace() * Matrix3s::Identity() - C; + } + + protected: + virtual void deleteBVs() = 0; + virtual bool allocateBVs() = 0; + + /// @brief Build the bounding volume hierarchy + virtual int buildTree() = 0; + + /// @brief Refit the bounding volume hierarchy + virtual int refitTree(bool bottomup) = 0; + + unsigned int num_tris_allocated; + unsigned int num_vertices_allocated; + unsigned int num_vertex_updated; /// for ccd vertex update + + protected: + /// \brief Comparison operators + virtual bool isEqual(const CollisionGeometry& other) const; +}; + +/// @brief A class describing the bounding hierarchy of a mesh model or a point +/// cloud model (which is viewed as a degraded version of mesh) \tparam BV one +/// of the bounding volume class in \ref Bounding_Volume. +template +class COAL_DLLAPI BVHModel : public BVHModelBase { + typedef BVHModelBase Base; + + public: + using bv_node_vector_t = + std::vector, Eigen::aligned_allocator>>; + + /// @brief Split rule to split one BV node into two children + shared_ptr> bv_splitter; + + /// @brief Fitting rule to fit a BV node to a set of geometry primitives + shared_ptr> bv_fitter; + + /// @brief Default constructor to build an empty BVH + BVHModel(); + + /// @brief Copy constructor from another BVH + /// + /// \param[in] other BVHModel to copy. + /// + BVHModel(const BVHModel& other); + + /// @brief Clone *this into a new BVHModel + virtual BVHModel* clone() const { return new BVHModel(*this); } + + /// @brief deconstruction, delete mesh data related. + ~BVHModel() {} + + /// @brief We provide getBV() and getNumBVs() because BVH may be compressed + /// (in future), so we must provide some flexibility here + + /// @brief Access the bv giving the its index + const BVNode& getBV(unsigned int i) const { + assert(i < num_bvs); + return (*bvs)[i]; + } + + /// @brief Access the bv giving the its index + BVNode& getBV(unsigned int i) { + assert(i < num_bvs); + return (*bvs)[i]; + } + + /// @brief Get the number of bv in the BVH + unsigned int getNumBVs() const { return num_bvs; } + + /// @brief Get the BV type: default is unknown + NODE_TYPE getNodeType() const { return BV_UNKNOWN; } + + /// @brief Check the number of memory used + int memUsage(const bool msg) const; + + /// @brief This is a special acceleration: BVH_model default stores the BV's + /// transform in world coordinate. However, we can also store each BV's + /// transform related to its parent BV node. When traversing the BVH, this can + /// save one matrix transformation. + void makeParentRelative() { + Matrix3s I(Matrix3s::Identity()); + makeParentRelativeRecurse(0, I, Vec3s::Zero()); + } + + protected: + void deleteBVs(); + bool allocateBVs(); + + unsigned int num_bvs_allocated; + std::shared_ptr> primitive_indices; + + /// @brief Bounding volume hierarchy + std::shared_ptr bvs; + + /// @brief Number of BV nodes in bounding volume hierarchy + unsigned int num_bvs; + + /// @brief Build the bounding volume hierarchy + int buildTree(); + + /// @brief Refit the bounding volume hierarchy + int refitTree(bool bottomup); + + /// @brief Refit the bounding volume hierarchy in a top-down way (slow but + /// more compact) + int refitTree_topdown(); + + /// @brief Refit the bounding volume hierarchy in a bottom-up way (fast but + /// less compact) + int refitTree_bottomup(); + + /// @brief Recursive kernel for hierarchy construction + int recursiveBuildTree(int bv_id, unsigned int first_primitive, + unsigned int num_primitives); + + /// @brief Recursive kernel for bottomup refitting + int recursiveRefitTree_bottomup(int bv_id); + + /// @ recursively compute each bv's transform related to its parent. For + /// default BV, only the translation works. For oriented BV (OBB, RSS, + /// OBBRSS), special implementation is provided. + void makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, + const Vec3s& parent_c) { + bv_node_vector_t& bvs_ = *bvs; + if (!bvs_[static_cast(bv_id)].isLeaf()) { + makeParentRelativeRecurse(bvs_[static_cast(bv_id)].first_child, + parent_axes, + bvs_[static_cast(bv_id)].getCenter()); + + makeParentRelativeRecurse( + bvs_[static_cast(bv_id)].first_child + 1, parent_axes, + bvs_[static_cast(bv_id)].getCenter()); + } + + bvs_[static_cast(bv_id)].bv = + translate(bvs_[static_cast(bv_id)].bv, -parent_c); + } + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const BVHModel* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const BVHModel& other = *other_ptr; + + bool res = Base::isEqual(other); + if (!res) return false; + + // unsigned int other_num_primitives = 0; + // if(other.primitive_indices) + // { + + // switch(other.getModelType()) + // { + // case BVH_MODEL_TRIANGLES: + // other_num_primitives = num_tris; + // break; + // case BVH_MODEL_POINTCLOUD: + // other_num_primitives = num_vertices; + // break; + // default: + // ; + // } + // } + + // unsigned int num_primitives = 0; + // if(primitive_indices) + // { + // + // switch(other.getModelType()) + // { + // case BVH_MODEL_TRIANGLES: + // num_primitives = num_tris; + // break; + // case BVH_MODEL_POINTCLOUD: + // num_primitives = num_vertices; + // break; + // default: + // ; + // } + // } + // + // if(num_primitives != other_num_primitives) + // return false; + // + // for(int k = 0; k < num_primitives; ++k) + // { + // if(primitive_indices[k] != other.primitive_indices[k]) + // return false; + // } + + if (num_bvs != other.num_bvs) return false; + + if ((!(bvs.get()) && other.bvs.get()) || (bvs.get() && !(other.bvs.get()))) + return false; + if (bvs.get() && other.bvs.get()) { + const bv_node_vector_t& bvs_ = *bvs; + const bv_node_vector_t& other_bvs_ = *(other.bvs); + for (unsigned int k = 0; k < num_bvs; ++k) { + if (bvs_[k] != other_bvs_[k]) return false; + } + } + + return true; + } +}; + +/// @} + +template <> +void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, + const Vec3s& parent_c); + +template <> +void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, + const Vec3s& parent_c); + +template <> +void BVHModel::makeParentRelativeRecurse(int bv_id, + Matrix3s& parent_axes, + const Vec3s& parent_c); + +/// @brief Specialization of getNodeType() for BVHModel with different BV types +template <> +NODE_TYPE BVHModel::getNodeType() const; + +template <> +NODE_TYPE BVHModel::getNodeType() const; + +template <> +NODE_TYPE BVHModel::getNodeType() const; + +template <> +NODE_TYPE BVHModel::getNodeType() const; + +template <> +NODE_TYPE BVHModel::getNodeType() const; + +template <> +NODE_TYPE BVHModel>::getNodeType() const; + +template <> +NODE_TYPE BVHModel>::getNodeType() const; + +template <> +NODE_TYPE BVHModel>::getNodeType() const; + +} // namespace coal + +#endif diff --git a/include/coal/BVH/BVH_utility.h b/include/coal/BVH/BVH_utility.h new file mode 100644 index 000000000..daf0e4781 --- /dev/null +++ b/include/coal/BVH/BVH_utility.h @@ -0,0 +1,115 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_BVH_UTILITY_H +#define COAL_BVH_UTILITY_H + +#include "coal/BVH/BVH_model.h" + +namespace coal { +/// @brief Extract the part of the BVHModel that is inside an AABB. +/// A triangle in collision with the AABB is considered inside. +template +COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3s& pose, const AABB& aabb); + +template <> +COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3s& pose, + const AABB& aabb); +template <> +COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3s& pose, + const AABB& aabb); +template <> +COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3s& pose, + const AABB& aabb); +template <> +COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3s& pose, + const AABB& aabb); +template <> +COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3s& pose, + const AABB& aabb); +template <> +COAL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, + const Transform3s& pose, + const AABB& aabb); +template <> +COAL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, + const Transform3s& pose, + const AABB& aabb); +template <> +COAL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, + const Transform3s& pose, + const AABB& aabb); + +/// @brief Compute the covariance matrix for a set or subset of points. if ts = +/// null, then indices refer to points directly; otherwise refer to triangles +COAL_DLLAPI void getCovariance(Vec3s* ps, Vec3s* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, + Matrix3s& M); + +/// @brief Compute the RSS bounding volume parameters: radius, rectangle size +/// and the origin, given the BV axises. +COAL_DLLAPI void getRadiusAndOriginAndRectangleSize( + Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, unsigned int n, + const Matrix3s& axes, Vec3s& origin, CoalScalar l[2], CoalScalar& r); + +/// @brief Compute the bounding volume extent and center for a set or subset of +/// points, given the BV axises. +COAL_DLLAPI void getExtentAndCenter(Vec3s* ps, Vec3s* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, + Matrix3s& axes, Vec3s& center, + Vec3s& extent); + +/// @brief Compute the center and radius for a triangle's circumcircle +COAL_DLLAPI void circumCircleComputation(const Vec3s& a, const Vec3s& b, + const Vec3s& c, Vec3s& center, + CoalScalar& radius); + +/// @brief Compute the maximum distance from a given center point to a point +/// cloud +COAL_DLLAPI CoalScalar maximumDistance(Vec3s* ps, Vec3s* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, + const Vec3s& query); + +} // namespace coal + +#endif diff --git a/include/coal/broadphase/broadphase.h b/include/coal/broadphase/broadphase.h new file mode 100644 index 000000000..887dbb141 --- /dev/null +++ b/include/coal/broadphase/broadphase.h @@ -0,0 +1,48 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Inria + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef COAL_BROADPHASE_BROADPHASE_H +#define COAL_BROADPHASE_BROADPHASE_H + +#include "coal/broadphase/broadphase_dynamic_AABB_tree.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "coal/broadphase/broadphase_bruteforce.h" +#include "coal/broadphase/broadphase_SaP.h" +#include "coal/broadphase/broadphase_SSaP.h" +#include "coal/broadphase/broadphase_interval_tree.h" +#include "coal/broadphase/broadphase_spatialhash.h" + +#include "coal/broadphase/default_broadphase_callbacks.h" + +#endif // ifndef COAL_BROADPHASE_BROADPHASE_H diff --git a/include/coal/broadphase/broadphase_SSaP.h b/include/coal/broadphase/broadphase_SSaP.h new file mode 100644 index 000000000..9ca05050c --- /dev/null +++ b/include/coal/broadphase/broadphase_SSaP.h @@ -0,0 +1,146 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROAD_PHASE_SSAP_H +#define COAL_BROAD_PHASE_SSAP_H + +#include +#include "coal/broadphase/broadphase_collision_manager.h" + +namespace coal { + +/// @brief Simple SAP collision manager +class COAL_DLLAPI SSaPCollisionManager : public BroadPhaseCollisionManager { + public: + typedef BroadPhaseCollisionManager Base; + using Base::getObjects; + + SSaPCollisionManager(); + + /// @brief remove one object from the manager + void registerObject(CollisionObject* obj); + + /// @brief add one object to the manager + void unregisterObject(CollisionObject* obj); + + /// @brief initialize the manager, related with the specific type of manager + void setup(); + + /// @brief update the condition of manager + virtual void update(); + + /// @brief clear the manager + void clear(); + + /// @brief return the objects managed by the manager + void getObjects(std::vector& objs) const; + + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; + + /// @brief perform distance computation between one object and all the objects + /// belonging to the manager + void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; + + /// @brief perform collision test for the objects belonging to the manager + /// (i.e., N^2 self collision) + void collide(CollisionCallBackBase* callback) const; + + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + void distance(DistanceCallBackBase* callback) const; + + /// @brief perform collision test with objects belonging to another manager + void collide(BroadPhaseCollisionManager* other_manager, + CollisionCallBackBase* callback) const; + + /// @brief perform distance test with objects belonging to another manager + void distance(BroadPhaseCollisionManager* other_manager, + DistanceCallBackBase* callback) const; + + /// @brief whether the manager is empty + bool empty() const; + + /// @brief the number of objects managed by the manager + size_t size() const; + + protected: + /// @brief check collision between one object and a list of objects, return + /// value is whether stop is possible + bool checkColl( + typename std::vector::const_iterator pos_start, + typename std::vector::const_iterator pos_end, + CollisionObject* obj, CollisionCallBackBase* callback) const; + + /// @brief check distance between one object and a list of objects, return + /// value is whether stop is possible + bool checkDis( + typename std::vector::const_iterator pos_start, + typename std::vector::const_iterator pos_end, + CollisionObject* obj, DistanceCallBackBase* callback, + CoalScalar& min_dist) const; + + bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; + + bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, + CoalScalar& min_dist) const; + + static int selectOptimalAxis( + const std::vector& objs_x, + const std::vector& objs_y, + const std::vector& objs_z, + typename std::vector::const_iterator& it_beg, + typename std::vector::const_iterator& it_end); + + /// @brief Objects sorted according to lower x value + std::vector objs_x; + + /// @brief Objects sorted according to lower y value + std::vector objs_y; + + /// @brief Objects sorted according to lower z value + std::vector objs_z; + + /// @brief tag about whether the environment is maintained suitably (i.e., the + /// objs_x, objs_y, objs_z are sorted correctly + bool setup_; +}; + +} // namespace coal + +#endif diff --git a/include/coal/broadphase/broadphase_SaP.h b/include/coal/broadphase/broadphase_SaP.h new file mode 100644 index 000000000..118ee65a9 --- /dev/null +++ b/include/coal/broadphase/broadphase_SaP.h @@ -0,0 +1,224 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROAD_PHASE_SAP_H +#define COAL_BROAD_PHASE_SAP_H + +#include +#include + +#include "coal/broadphase/broadphase_collision_manager.h" + +namespace coal { + +/// @brief Rigorous SAP collision manager +class COAL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager { + public: + typedef BroadPhaseCollisionManager Base; + using Base::getObjects; + + SaPCollisionManager(); + + ~SaPCollisionManager(); + + /// @brief add objects to the manager + void registerObjects(const std::vector& other_objs); + + /// @brief remove one object from the manager + void registerObject(CollisionObject* obj); + + /// @brief add one object to the manager + void unregisterObject(CollisionObject* obj); + + /// @brief initialize the manager, related with the specific type of manager + void setup(); + + /// @brief update the condition of manager + virtual void update(); + + /// @brief update the manager by explicitly given the object updated + void update(CollisionObject* updated_obj); + + /// @brief update the manager by explicitly given the set of objects update + void update(const std::vector& updated_objs); + + /// @brief clear the manager + void clear(); + + /// @brief return the objects managed by the manager + void getObjects(std::vector& objs) const; + + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; + + /// @brief perform distance computation between one object and all the objects + /// belonging to the manager + void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; + + /// @brief perform collision test for the objects belonging to the manager + /// (i.e., N^2 self collision) + void collide(CollisionCallBackBase* callback) const; + + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + void distance(DistanceCallBackBase* callback) const; + + /// @brief perform collision test with objects belonging to another manager + void collide(BroadPhaseCollisionManager* other_manager, + CollisionCallBackBase* callback) const; + + /// @brief perform distance test with objects belonging to another manager + void distance(BroadPhaseCollisionManager* other_manager, + DistanceCallBackBase* callback) const; + + /// @brief whether the manager is empty + bool empty() const; + + /// @brief the number of objects managed by the manager + size_t size() const; + + protected: + struct EndPoint; + + /// @brief SAP interval for one object + struct SaPAABB { + /// @brief object + CollisionObject* obj; + + /// @brief lower bound end point of the interval + EndPoint* lo; + + /// @brief higher bound end point of the interval + EndPoint* hi; + + /// @brief cached AABB value + AABB cached; + }; + + /// @brief End point for an interval + struct EndPoint { + /// @brief tag for whether it is a lower bound or higher bound of an + /// interval, 0 for lo, and 1 for hi + char minmax; + + /// @brief back pointer to SAP interval + SaPAABB* aabb; + + /// @brief the previous end point in the end point list + EndPoint* prev[3]; + + /// @brief the next end point in the end point list + EndPoint* next[3]; + + /// @brief get the value of the end point + const Vec3s& getVal() const; + + /// @brief set the value of the end point + Vec3s& getVal(); + + CoalScalar getVal(int i) const; + + CoalScalar& getVal(int i); + }; + + /// @brief A pair of objects that are not culling away and should further + /// check collision + struct SaPPair { + SaPPair(CollisionObject* a, CollisionObject* b); + + CollisionObject* obj1; + CollisionObject* obj2; + + bool operator==(const SaPPair& other) const; + }; + + /// @brief Functor to help unregister one object + class COAL_DLLAPI isUnregistered { + CollisionObject* obj; + + public: + isUnregistered(CollisionObject* obj_); + + bool operator()(const SaPPair& pair) const; + }; + + /// @brief Functor to help remove collision pairs no longer valid (i.e., + /// should be culled away) + class COAL_DLLAPI isNotValidPair { + CollisionObject* obj1; + CollisionObject* obj2; + + public: + isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_); + + bool operator()(const SaPPair& pair); + }; + + void update_(SaPAABB* updated_aabb); + + void updateVelist(); + + /// @brief End point list for x, y, z coordinates + EndPoint* elist[3]; + + /// @brief vector version of elist, for acceleration + std::vector velist[3]; + + /// @brief SAP interval list + std::list AABB_arr; + + /// @brief The pair of objects that should further check for collision + std::list overlap_pairs; + + int optimal_axis; + + std::map obj_aabb_map; + + bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, + CoalScalar& min_dist) const; + + bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; + + void addToOverlapPairs(const SaPPair& p); + + void removeFromOverlapPairs(const SaPPair& p); +}; + +} // namespace coal + +#endif diff --git a/include/coal/broadphase/broadphase_bruteforce.h b/include/coal/broadphase/broadphase_bruteforce.h new file mode 100644 index 000000000..ab1f9d564 --- /dev/null +++ b/include/coal/broadphase/broadphase_bruteforce.h @@ -0,0 +1,112 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROAD_PHASE_BRUTE_FORCE_H +#define COAL_BROAD_PHASE_BRUTE_FORCE_H + +#include +#include "coal/broadphase/broadphase_collision_manager.h" + +namespace coal { + +/// @brief Brute force N-body collision manager +class COAL_DLLAPI NaiveCollisionManager : public BroadPhaseCollisionManager { + public: + typedef BroadPhaseCollisionManager Base; + using Base::getObjects; + + NaiveCollisionManager(); + + /// @brief add objects to the manager + void registerObjects(const std::vector& other_objs); + + /// @brief add one object to the manager + void registerObject(CollisionObject* obj); + + /// @brief remove one object from the manager + void unregisterObject(CollisionObject* obj); + + /// @brief initialize the manager, related with the specific type of manager + void setup(); + + /// @brief update the condition of manager + virtual void update(); + + /// @brief clear the manager + void clear(); + + /// @brief return the objects managed by the manager + void getObjects(std::vector& objs) const; + + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; + + /// @brief perform distance computation between one object and all the objects + /// belonging to the manager + void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; + + /// @brief perform collision test for the objects belonging to the manager + /// (i.e., N^2 self collision) + void collide(CollisionCallBackBase* callback) const; + + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + void distance(DistanceCallBackBase* callback) const; + + /// @brief perform collision test with objects belonging to another manager + void collide(BroadPhaseCollisionManager* other_manager, + CollisionCallBackBase* callback) const; + + /// @brief perform distance test with objects belonging to another manager + void distance(BroadPhaseCollisionManager* other_manager, + DistanceCallBackBase* callback) const; + + /// @brief whether the manager is empty + bool empty() const; + + /// @brief the number of objects managed by the manager + size_t size() const; + + protected: + /// @brief objects belonging to the manager are stored in a list structure + std::list objs; +}; + +} // namespace coal + +#endif diff --git a/include/coal/broadphase/broadphase_callbacks.h b/include/coal/broadphase/broadphase_callbacks.h new file mode 100644 index 000000000..5a132d106 --- /dev/null +++ b/include/coal/broadphase/broadphase_callbacks.h @@ -0,0 +1,96 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Justin Carpentier (justin.carpentier@inria.fr) */ + +#ifndef COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H +#define COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H + +#include "coal/fwd.hh" +#include "coal/data_types.h" + +namespace coal { + +/// @brief Base callback class for collision queries. +/// This class can be supersed by child classes to provide desired behaviors +/// according to the application (e.g, only listing the potential +/// CollisionObjects in collision). +struct COAL_DLLAPI CollisionCallBackBase { + /// @brief Initialization of the callback before running the collision + /// broadphase manager. + virtual void init() {}; + + /// @brief Collision evaluation between two objects in collision. + /// This callback will cause the broadphase evaluation to stop if it + /// returns true. + /// + /// @param[in] o1 Collision object #1. + /// @param[in] o2 Collision object #2. + virtual bool collide(CollisionObject* o1, CollisionObject* o2) = 0; + + /// @brief Functor call associated to the collide operation. + virtual bool operator()(CollisionObject* o1, CollisionObject* o2) { + return collide(o1, o2); + } +}; + +/// @brief Base callback class for distance queries. +/// This class can be supersed by child classes to provide desired behaviors +/// according to the application (e.g, only listing the potential +/// CollisionObjects in collision). +struct COAL_DLLAPI DistanceCallBackBase { + /// @brief Initialization of the callback before running the collision + /// broadphase manager. + virtual void init() {}; + + /// @brief Distance evaluation between two objects in collision. + /// This callback will cause the broadphase evaluation to stop if it + /// returns true. + /// + /// @param[in] o1 Collision object #1. + /// @param[in] o2 Collision object #2. + /// @param[out] dist Distance between the two collision geometries. + virtual bool distance(CollisionObject* o1, CollisionObject* o2, + CoalScalar& dist) = 0; + + /// @brief Functor call associated to the distance operation. + virtual bool operator()(CollisionObject* o1, CollisionObject* o2, + CoalScalar& dist) { + return distance(o1, o2, dist); + } +}; + +} // namespace coal + +#endif // COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H diff --git a/include/coal/broadphase/broadphase_collision_manager.h b/include/coal/broadphase/broadphase_collision_manager.h new file mode 100644 index 000000000..a394878af --- /dev/null +++ b/include/coal/broadphase/broadphase_collision_manager.h @@ -0,0 +1,139 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H +#define COAL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H + +#include +#include +#include + +#include "coal/collision_object.h" +#include "coal/broadphase/broadphase_callbacks.h" + +namespace coal { + +/// @brief Base class for broad phase collision. It helps to accelerate the +/// collision/distance between N objects. Also support self collision, self +/// distance and collision/distance with another M objects. +class COAL_DLLAPI BroadPhaseCollisionManager { + public: + BroadPhaseCollisionManager(); + + virtual ~BroadPhaseCollisionManager(); + + /// @brief add objects to the manager + virtual void registerObjects(const std::vector& other_objs); + + /// @brief add one object to the manager + virtual void registerObject(CollisionObject* obj) = 0; + + /// @brief remove one object from the manager + virtual void unregisterObject(CollisionObject* obj) = 0; + + /// @brief initialize the manager, related with the specific type of manager + virtual void setup() = 0; + + /// @brief update the condition of manager + virtual void update() = 0; + + /// @brief update the manager by explicitly given the object updated + virtual void update(CollisionObject* updated_obj); + + /// @brief update the manager by explicitly given the set of objects update + virtual void update(const std::vector& updated_objs); + + /// @brief clear the manager + virtual void clear() = 0; + + /// @brief return the objects managed by the manager + virtual void getObjects(std::vector& objs) const = 0; + + /// @brief return the objects managed by the manager + virtual std::vector getObjects() const { + std::vector res(size()); + getObjects(res); + return res; + }; + + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + virtual void collide(CollisionObject* obj, + CollisionCallBackBase* callback) const = 0; + + /// @brief perform distance computation between one object and all the objects + /// belonging to the manager + virtual void distance(CollisionObject* obj, + DistanceCallBackBase* callback) const = 0; + + /// @brief perform collision test for the objects belonging to the manager + /// (i.e., N^2 self collision) + virtual void collide(CollisionCallBackBase* callback) const = 0; + + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + virtual void distance(DistanceCallBackBase* callback) const = 0; + + /// @brief perform collision test with objects belonging to another manager + virtual void collide(BroadPhaseCollisionManager* other_manager, + CollisionCallBackBase* callback) const = 0; + + /// @brief perform distance test with objects belonging to another manager + virtual void distance(BroadPhaseCollisionManager* other_manager, + DistanceCallBackBase* callback) const = 0; + + /// @brief whether the manager is empty + virtual bool empty() const = 0; + + /// @brief the number of objects managed by the manager + virtual size_t size() const = 0; + + protected: + /// @brief tools help to avoid repeating collision or distance callback for + /// the pairs of objects tested before. It can be useful for some of the + /// broadphase algorithms. + mutable std::set > tested_set; + mutable bool enable_tested_set_; + + bool inTestedSet(CollisionObject* a, CollisionObject* b) const; + + void insertTestedSet(CollisionObject* a, CollisionObject* b) const; +}; + +} // namespace coal + +#endif diff --git a/include/coal/broadphase/broadphase_continuous_collision_manager-inl.h b/include/coal/broadphase/broadphase_continuous_collision_manager-inl.h new file mode 100644 index 000000000..b3e7a4aa8 --- /dev/null +++ b/include/coal/broadphase/broadphase_continuous_collision_manager-inl.h @@ -0,0 +1,84 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H +#define COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H + +#include "coal/broadphase/broadphase_continuous_collision_manager.h" +#include + +namespace coal { + +//============================================================================== +BroadPhaseContinuousCollisionManager::BroadPhaseContinuousCollisionManager() { + // Do nothing +} + +//============================================================================== + +BroadPhaseContinuousCollisionManager::~BroadPhaseContinuousCollisionManager() { + // Do nothing +} + +//============================================================================== + +void BroadPhaseContinuousCollisionManager::registerObjects( + const std::vector& other_objs) { + for (size_t i = 0; i < other_objs.size(); ++i) registerObject(other_objs[i]); +} + +//============================================================================== + +void BroadPhaseContinuousCollisionManager::update( + ContinuousCollisionObject* updated_obj) { + COAL_UNUSED_VARIABLE(updated_obj); + + update(); +} + +//============================================================================== + +void BroadPhaseContinuousCollisionManager::update( + const std::vector& updated_objs) { + COAL_UNUSED_VARIABLE(updated_objs); + + update(); +} + +} // namespace coal + +#endif diff --git a/include/coal/broadphase/broadphase_continuous_collision_manager.h b/include/coal/broadphase/broadphase_continuous_collision_manager.h new file mode 100644 index 000000000..c2e36e746 --- /dev/null +++ b/include/coal/broadphase/broadphase_continuous_collision_manager.h @@ -0,0 +1,143 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H +#define COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H + +#include "coal/broadphase/broadphase_collision_manager.h" +#include "coal/collision_object.h" +#include "coal/narrowphase/continuous_collision_object.h" + +namespace coal { + +/// @brief Callback for continuous collision between two objects. Return value +/// is whether can stop now. +template +using ContinuousCollisionCallBack = bool (*)(ContinuousCollisionObject* o1, + ContinuousCollisionObject* o2, + void* cdata); + +/// @brief Callback for continuous distance between two objects, Return value is +/// whether can stop now, also return the minimum distance till now. +template +using ContinuousDistanceCallBack = bool (*)(ContinuousCollisionObject* o1, + ContinuousCollisionObject* o2, + S& dist); + +/// @brief Base class for broad phase continuous collision. It helps to +/// accelerate the continuous collision/distance between N objects. Also support +/// self collision, self distance and collision/distance with another M objects. +template +class COAL_DLLAPI BroadPhaseContinuousCollisionManager { + public: + BroadPhaseContinuousCollisionManager(); + + virtual ~BroadPhaseContinuousCollisionManager(); + + /// @brief add objects to the manager + virtual void registerObjects( + const std::vector& other_objs); + + /// @brief add one object to the manager + virtual void registerObject(ContinuousCollisionObject* obj) = 0; + + /// @brief remove one object from the manager + virtual void unregisterObject(ContinuousCollisionObject* obj) = 0; + + /// @brief initialize the manager, related with the specific type of manager + virtual void setup() = 0; + + /// @brief update the condition of manager + virtual void update() = 0; + + /// @brief update the manager by explicitly given the object updated + virtual void update(ContinuousCollisionObject* updated_obj); + + /// @brief update the manager by explicitly given the set of objects update + virtual void update( + const std::vector& updated_objs); + + /// @brief clear the manager + virtual void clear() = 0; + + /// @brief return the objects managed by the manager + virtual void getObjects( + std::vector& objs) const = 0; + + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + virtual void collide(ContinuousCollisionObject* obj, + CollisionCallBackBase* callback) const = 0; + + /// @brief perform distance computation between one object and all the objects + /// belonging to the manager + virtual void distance(ContinuousCollisionObject* obj, + DistanceCallBackBase* callback) const = 0; + + /// @brief perform collision test for the objects belonging to the manager + /// (i.e., N^2 self collision) + virtual void collide(CollisionCallBackBase* callback) const = 0; + + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + virtual void distance(DistanceCallBackBase* callback) const = 0; + + /// @brief perform collision test with objects belonging to another manager + virtual void collide(BroadPhaseContinuousCollisionManager* other_manager, + CollisionCallBackBase* callback) const = 0; + + /// @brief perform distance test with objects belonging to another manager + virtual void distance(BroadPhaseContinuousCollisionManager* other_manager, + DistanceCallBackBase* callback) const = 0; + + /// @brief whether the manager is empty + virtual bool empty() const = 0; + + /// @brief the number of objects managed by the manager + virtual size_t size() const = 0; +}; + +using BroadPhaseContinuousCollisionManagerf = + BroadPhaseContinuousCollisionManager; +using BroadPhaseContinuousCollisionManagerd = + BroadPhaseContinuousCollisionManager; + +} // namespace coal + +#include "coal/broadphase/broadphase_continuous_collision_manager-inl.h" + +#endif diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h new file mode 100644 index 000000000..6e1314f6e --- /dev/null +++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h @@ -0,0 +1,234 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H +#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H + +#include "coal/broadphase/broadphase_dynamic_AABB_tree.h" + +#include + +#if COAL_HAVE_OCTOMAP +#include "coal/octree.h" +#endif + +#include "coal/BV/BV.h" +#include "coal/shape/geometric_shapes_utility.h" + +namespace coal { +namespace detail { + +namespace dynamic_AABB_tree { + +#if COAL_HAVE_OCTOMAP + +//============================================================================== +template +bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, + const OcTree* tree2, const OcTree::OcTreeNode* root2, + const AABB& root2_bv, + const Eigen::MatrixBase& translation2, + CollisionCallBackBase* callback) { + if (!root2) { + if (root1->isLeaf()) { + CollisionObject* obj1 = static_cast(root1->data); + + if (!obj1->collisionGeometry()->isFree()) { + const AABB& root2_bv_t = translate(root2_bv, translation2); + if (root1->bv.overlap(root2_bv_t)) { + Box* box = new Box(); + Transform3s box_tf; + Transform3s tf2 = Transform3s::Identity(); + tf2.translation() = translation2; + constructBox(root2_bv, tf2, *box, box_tf); + + box->cost_density = + tree2->getOccupancyThres(); // thresholds are 0, 1, so uncertain + + CollisionObject obj2(shared_ptr(box), box_tf); + return (*callback)(obj1, &obj2); + } + } + } else { + if (collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv, + translation2, callback)) + return true; + if (collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv, + translation2, callback)) + return true; + } + + return false; + } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { + CollisionObject* obj1 = static_cast(root1->data); + + if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { + const AABB& root2_bv_t = translate(root2_bv, translation2); + if (root1->bv.overlap(root2_bv_t)) { + Box* box = new Box(); + Transform3s box_tf; + Transform3s tf2 = Transform3s::Identity(); + tf2.translation() = translation2; + constructBox(root2_bv, tf2, *box, box_tf); + + box->cost_density = root2->getOccupancy(); + box->threshold_occupied = tree2->getOccupancyThres(); + + CollisionObject obj2(shared_ptr(box), box_tf); + return (*callback)(obj1, &obj2); + } else + return false; + } else + return false; + } + + const AABB& root2_bv_t = translate(root2_bv, translation2); + if (tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false; + + if (!tree2->nodeHasChildren(root2) || + (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { + if (collisionRecurse_(root1->children[0], tree2, root2, root2_bv, + translation2, callback)) + return true; + if (collisionRecurse_(root1->children[1], tree2, root2, root2_bv, + translation2, callback)) + return true; + } else { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + + if (collisionRecurse_(root1, tree2, child, child_bv, translation2, + callback)) + return true; + } else { + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + if (collisionRecurse_(root1, tree2, nullptr, child_bv, translation2, + callback)) + return true; + } + } + } + return false; +} + +//============================================================================== +template +bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, + const OcTree* tree2, const OcTree::OcTreeNode* root2, + const AABB& root2_bv, + const Eigen::MatrixBase& translation2, + DistanceCallBackBase* callback, CoalScalar& min_dist) { + if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { + if (tree2->isNodeOccupied(root2)) { + Box* box = new Box(); + Transform3s box_tf; + Transform3s tf2 = Transform3s::Identity(); + tf2.translation() = translation2; + constructBox(root2_bv, tf2, *box, box_tf); + CollisionObject obj(shared_ptr(box), box_tf); + return (*callback)(static_cast(root1->data), &obj, + min_dist); + } else + return false; + } + + if (!tree2->isNodeOccupied(root2)) return false; + + if (!tree2->nodeHasChildren(root2) || + (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { + const AABB& aabb2 = translate(root2_bv, translation2); + CoalScalar d1 = aabb2.distance(root1->children[0]->bv); + CoalScalar d2 = aabb2.distance(root1->children[1]->bv); + + if (d2 < d1) { + if (d2 < min_dist) { + if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv, + translation2, callback, min_dist)) + return true; + } + + if (d1 < min_dist) { + if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv, + translation2, callback, min_dist)) + return true; + } + } else { + if (d1 < min_dist) { + if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv, + translation2, callback, min_dist)) + return true; + } + + if (d2 < min_dist) { + if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv, + translation2, callback, min_dist)) + return true; + } + } + } else { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + const AABB& aabb2 = translate(child_bv, translation2); + + CoalScalar d = root1->bv.distance(aabb2); + + if (d < min_dist) { + if (distanceRecurse_(root1, tree2, child, child_bv, translation2, + callback, min_dist)) + return true; + } + } + } + } + + return false; +} + +#endif + +} // namespace dynamic_AABB_tree +} // namespace detail +} // namespace coal + +#endif diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree.h new file mode 100644 index 000000000..d3ecd5497 --- /dev/null +++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree.h @@ -0,0 +1,150 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_H +#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_H + +#include +#include + +#include "coal/fwd.hh" +// #include "coal/BV/utility.h" +#include "coal/shape/geometric_shapes.h" +// #include "coal/geometry/shape/utility.h" +#include "coal/broadphase/broadphase_collision_manager.h" +#include "coal/broadphase/detail/hierarchy_tree.h" + +namespace coal { + +class COAL_DLLAPI DynamicAABBTreeCollisionManager + : public BroadPhaseCollisionManager { + public: + typedef BroadPhaseCollisionManager Base; + using Base::getObjects; + + using DynamicAABBNode = detail::NodeBase; + using DynamicAABBTable = + std::unordered_map; + + int max_tree_nonbalanced_level; + int tree_incremental_balance_pass; + int* tree_topdown_balance_threshold{nullptr}; + int* tree_topdown_level{nullptr}; + int tree_init_level; + + bool octree_as_geometry_collide; + bool octree_as_geometry_distance; + + DynamicAABBTreeCollisionManager(); + + /// @brief add objects to the manager + void registerObjects(const std::vector& other_objs); + + /// @brief add one object to the manager + void registerObject(CollisionObject* obj); + + /// @brief remove one object from the manager + void unregisterObject(CollisionObject* obj); + + /// @brief initialize the manager, related with the specific type of manager + void setup(); + + /// @brief update the condition of manager + virtual void update(); + + /// @brief update the manager by explicitly given the object updated + void update(CollisionObject* updated_obj); + + /// @brief update the manager by explicitly given the set of objects update + void update(const std::vector& updated_objs); + + /// @brief clear the manager + void clear(); + + /// @brief return the objects managed by the manager + void getObjects(std::vector& objs) const; + + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; + + /// @brief perform distance computation between one object and all the objects + /// belonging to the manager + void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; + + /// @brief perform collision test for the objects belonging to the manager + /// (i.e., N^2 self collision) + void collide(CollisionCallBackBase* callback) const; + + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + void distance(DistanceCallBackBase* callback) const; + + /// @brief perform collision test with objects belonging to another manager + void collide(BroadPhaseCollisionManager* other_manager_, + CollisionCallBackBase* callback) const; + + /// @brief perform distance test with objects belonging to another manager + void distance(BroadPhaseCollisionManager* other_manager_, + DistanceCallBackBase* callback) const; + + /// @brief whether the manager is empty + bool empty() const; + + /// @brief the number of objects managed by the manager + size_t size() const; + + /// @brief returns the AABB tree structure. + const detail::HierarchyTree& getTree() const; + + /// @brief returns the AABB tree structure. + detail::HierarchyTree& getTree(); + + private: + detail::HierarchyTree dtree{}; + std::unordered_map table; + + bool setup_; + + void update_(CollisionObject* updated_obj); +}; + +} // namespace coal + +#include "coal/broadphase/broadphase_dynamic_AABB_tree-inl.h" + +#endif diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h new file mode 100644 index 000000000..730db5782 --- /dev/null +++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h @@ -0,0 +1,233 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H +#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H + +#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "coal/shape/geometric_shapes_utility.h" + +#if COAL_HAVE_OCTOMAP +#include "coal/octree.h" +#endif + +namespace coal { +namespace detail { +namespace dynamic_AABB_tree_array { + +#if COAL_HAVE_OCTOMAP + +//============================================================================== +template +bool collisionRecurse_( + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, + size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, + const AABB& root2_bv, const Eigen::MatrixBase& translation2, + CollisionCallBackBase* callback) { + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = + nodes1 + root1_id; + if (!root2) { + if (root1->isLeaf()) { + CollisionObject* obj1 = static_cast(root1->data); + + if (!obj1->collisionGeometry()->isFree()) { + const AABB& root_bv_t = translate(root2_bv, translation2); + if (root1->bv.overlap(root_bv_t)) { + Box* box = new Box(); + Transform3s box_tf; + Transform3s tf2 = Transform3s::Identity(); + tf2.translation() = translation2; + constructBox(root2_bv, tf2, *box, box_tf); + + box->cost_density = tree2->getDefaultOccupancy(); + + CollisionObject obj2(shared_ptr(box), box_tf); + return (*callback)(obj1, &obj2); + } + } + } else { + if (collisionRecurse_(nodes1, root1->children[0], tree2, nullptr, + root2_bv, translation2, callback)) + return true; + if (collisionRecurse_(nodes1, root1->children[1], tree2, nullptr, + root2_bv, translation2, callback)) + return true; + } + + return false; + } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { + CollisionObject* obj1 = static_cast(root1->data); + if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { + const AABB& root_bv_t = translate(root2_bv, translation2); + if (root1->bv.overlap(root_bv_t)) { + Box* box = new Box(); + Transform3s box_tf; + Transform3s tf2 = Transform3s::Identity(); + tf2.translation() = translation2; + constructBox(root2_bv, tf2, *box, box_tf); + + box->cost_density = root2->getOccupancy(); + box->threshold_occupied = tree2->getOccupancyThres(); + + CollisionObject obj2(shared_ptr(box), box_tf); + return (*callback)(obj1, &obj2); + } else + return false; + } else + return false; + } + + const AABB& root_bv_t = translate(root2_bv, translation2); + if (tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) return false; + + if (!tree2->nodeHasChildren(root2) || + (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { + if (collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, + translation2, callback)) + return true; + if (collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, + translation2, callback)) + return true; + } else { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + + if (collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, + translation2, callback)) + return true; + } else { + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + if (collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv, + translation2, callback)) + return true; + } + } + } + + return false; +} + +//============================================================================== +template +bool distanceRecurse_( + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, + size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, + const AABB& root2_bv, const Eigen::MatrixBase& translation2, + DistanceCallBackBase* callback, CoalScalar& min_dist) { + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = + nodes1 + root1_id; + if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { + if (tree2->isNodeOccupied(root2)) { + Box* box = new Box(); + Transform3s box_tf; + Transform3s tf2 = Transform3s::Identity(); + tf2.translation() = translation2; + constructBox(root2_bv, tf2, *box, box_tf); + CollisionObject obj(shared_ptr(box), box_tf); + return (*callback)(static_cast(root1->data), &obj, + min_dist); + } else + return false; + } + + if (!tree2->isNodeOccupied(root2)) return false; + + if (!tree2->nodeHasChildren(root2) || + (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { + const AABB& aabb2 = translate(root2_bv, translation2); + + CoalScalar d1 = aabb2.distance((nodes1 + root1->children[0])->bv); + CoalScalar d2 = aabb2.distance((nodes1 + root1->children[1])->bv); + + if (d2 < d1) { + if (d2 < min_dist) { + if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, + translation2, callback, min_dist)) + return true; + } + + if (d1 < min_dist) { + if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, + translation2, callback, min_dist)) + return true; + } + } else { + if (d1 < min_dist) { + if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, + translation2, callback, min_dist)) + return true; + } + + if (d2 < min_dist) { + if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, + translation2, callback, min_dist)) + return true; + } + } + } else { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + + const AABB& aabb2 = translate(child_bv, translation2); + CoalScalar d = root1->bv.distance(aabb2); + + if (d < min_dist) { + if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, + translation2, callback, min_dist)) + return true; + } + } + } + } + + return false; +} + +#endif + +} // namespace dynamic_AABB_tree_array +} // namespace detail +} // namespace coal + +#endif diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h new file mode 100644 index 000000000..35f17cd82 --- /dev/null +++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h @@ -0,0 +1,146 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H +#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H + +#include +#include +#include + +#include "coal/fwd.hh" +// #include "coal/BV/utility.h" +#include "coal/shape/geometric_shapes.h" +// #include "coal/geometry/shape/utility.h" +#include "coal/broadphase/broadphase_collision_manager.h" +#include "coal/broadphase/detail/hierarchy_tree_array.h" + +namespace coal { + +class COAL_DLLAPI DynamicAABBTreeArrayCollisionManager + : public BroadPhaseCollisionManager { + public: + typedef BroadPhaseCollisionManager Base; + using Base::getObjects; + + using DynamicAABBNode = detail::implementation_array::NodeBase; + using DynamicAABBTable = std::unordered_map; + + int max_tree_nonbalanced_level; + int tree_incremental_balance_pass; + int* tree_topdown_balance_threshold{nullptr}; + int* tree_topdown_level{nullptr}; + int tree_init_level; + + bool octree_as_geometry_collide; + bool octree_as_geometry_distance; + + DynamicAABBTreeArrayCollisionManager(); + + /// @brief add objects to the manager + void registerObjects(const std::vector& other_objs); + + /// @brief add one object to the manager + void registerObject(CollisionObject* obj); + + /// @brief remove one object from the manager + void unregisterObject(CollisionObject* obj); + + /// @brief initialize the manager, related with the specific type of manager + void setup(); + + /// @brief update the condition of manager + virtual void update(); + + /// @brief update the manager by explicitly given the object updated + void update(CollisionObject* updated_obj); + + /// @brief update the manager by explicitly given the set of objects update + void update(const std::vector& updated_objs); + + /// @brief clear the manager + void clear(); + + /// @brief return the objects managed by the manager + void getObjects(std::vector& objs) const; + + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; + + /// @brief perform distance computation between one object and all the objects + /// belonging to the manager + void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; + + /// @brief perform collision test for the objects belonging to the manager + /// (i.e., N^2 self collision) + void collide(CollisionCallBackBase* callback) const; + + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + void distance(DistanceCallBackBase* callback) const; + + /// @brief perform collision test with objects belonging to another manager + void collide(BroadPhaseCollisionManager* other_manager_, + CollisionCallBackBase* callback) const; + + /// @brief perform distance test with objects belonging to another manager + void distance(BroadPhaseCollisionManager* other_manager_, + DistanceCallBackBase* callback) const; + + /// @brief whether the manager is empty + bool empty() const; + + /// @brief the number of objects managed by the manager + size_t size() const; + + const detail::implementation_array::HierarchyTree& getTree() const; + + private: + detail::implementation_array::HierarchyTree dtree{}; + std::unordered_map table; + + bool setup_; + + void update_(CollisionObject* updated_obj); +}; + +} // namespace coal + +#include "coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h" + +#endif diff --git a/include/coal/broadphase/broadphase_interval_tree.h b/include/coal/broadphase/broadphase_interval_tree.h new file mode 100644 index 000000000..9950cf82e --- /dev/null +++ b/include/coal/broadphase/broadphase_interval_tree.h @@ -0,0 +1,169 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROAD_PHASE_INTERVAL_TREE_H +#define COAL_BROAD_PHASE_INTERVAL_TREE_H + +#include +#include + +#include "coal/broadphase/broadphase_collision_manager.h" +#include "coal/broadphase/detail/interval_tree.h" + +namespace coal { + +/// @brief Collision manager based on interval tree +class COAL_DLLAPI IntervalTreeCollisionManager + : public BroadPhaseCollisionManager { + public: + typedef BroadPhaseCollisionManager Base; + using Base::getObjects; + + IntervalTreeCollisionManager(); + + ~IntervalTreeCollisionManager(); + + /// @brief remove one object from the manager + void registerObject(CollisionObject* obj); + + /// @brief add one object to the manager + void unregisterObject(CollisionObject* obj); + + /// @brief initialize the manager, related with the specific type of manager + void setup(); + + /// @brief update the condition of manager + virtual void update(); + + /// @brief update the manager by explicitly given the object updated + void update(CollisionObject* updated_obj); + + /// @brief update the manager by explicitly given the set of objects update + void update(const std::vector& updated_objs); + + /// @brief clear the manager + void clear(); + + /// @brief return the objects managed by the manager + void getObjects(std::vector& objs) const; + + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; + + /// @brief perform distance computation between one object and all the objects + /// belonging to the manager + void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; + + /// @brief perform collision test for the objects belonging to the manager + /// (i.e., N^2 self collision) + void collide(CollisionCallBackBase* callback) const; + + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + void distance(DistanceCallBackBase* callback) const; + + /// @brief perform collision test with objects belonging to another manager + void collide(BroadPhaseCollisionManager* other_manager, + CollisionCallBackBase* callback) const; + + /// @brief perform distance test with objects belonging to another manager + void distance(BroadPhaseCollisionManager* other_manager, + DistanceCallBackBase* callback) const; + + /// @brief whether the manager is empty + bool empty() const; + + /// @brief the number of objects managed by the manager + size_t size() const; + + protected: + /// @brief SAP end point + /// @brief SAP end point + struct COAL_DLLAPI EndPoint { + /// @brief object related with the end point + CollisionObject* obj; + + /// @brief end point value + CoalScalar value; + + /// @brief tag for whether it is a lower bound or higher bound of an + /// interval, 0 for lo, and 1 for hi + char minmax; + + bool operator<(const EndPoint& p) const; + }; + + /// @brief Extention interval tree's interval to SAP interval, adding more + /// information + struct COAL_DLLAPI SAPInterval : public detail::SimpleInterval { + CollisionObject* obj; + + SAPInterval(CoalScalar low_, CoalScalar high_, CollisionObject* obj_); + }; + + bool checkColl( + typename std::deque::const_iterator pos_start, + typename std::deque::const_iterator pos_end, + CollisionObject* obj, CollisionCallBackBase* callback) const; + + bool checkDist( + typename std::deque::const_iterator pos_start, + typename std::deque::const_iterator pos_end, + CollisionObject* obj, DistanceCallBackBase* callback, + CoalScalar& min_dist) const; + + bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; + + bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, + CoalScalar& min_dist) const; + + /// @brief vector stores all the end points + std::vector endpoints[3]; + + /// @brief interval tree manages the intervals + detail::IntervalTree* interval_trees[3]; + + std::map obj_interval_maps[3]; + + /// @brief tag for whether the interval tree is maintained suitably + bool setup_; +}; + +} // namespace coal + +#endif diff --git a/include/coal/broadphase/broadphase_spatialhash-inl.h b/include/coal/broadphase/broadphase_spatialhash-inl.h new file mode 100644 index 000000000..9f155a4e0 --- /dev/null +++ b/include/coal/broadphase/broadphase_spatialhash-inl.h @@ -0,0 +1,539 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H +#define COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H + +#include "coal/broadphase/broadphase_spatialhash.h" + +namespace coal { + +//============================================================================== +template +SpatialHashingCollisionManager::SpatialHashingCollisionManager( + CoalScalar cell_size, const Vec3s& scene_min, const Vec3s& scene_max, + unsigned int default_table_size) + : scene_limit(AABB(scene_min, scene_max)), + hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) { + hash_table->init(default_table_size); +} + +//============================================================================== +template +SpatialHashingCollisionManager::~SpatialHashingCollisionManager() { + delete hash_table; +} + +//============================================================================== +template +void SpatialHashingCollisionManager::registerObject( + CollisionObject* obj) { + objs.push_back(obj); + + const AABB& obj_aabb = obj->getAABB(); + AABB overlap_aabb; + + if (scene_limit.overlap(obj_aabb, overlap_aabb)) { + if (!scene_limit.contain(obj_aabb)) + objs_partially_penetrating_scene_limit.push_back(obj); + + hash_table->insert(overlap_aabb, obj); + } else { + objs_outside_scene_limit.push_back(obj); + } + + obj_aabb_map[obj] = obj_aabb; +} + +//============================================================================== +template +void SpatialHashingCollisionManager::unregisterObject( + CollisionObject* obj) { + objs.remove(obj); + + const AABB& obj_aabb = obj->getAABB(); + AABB overlap_aabb; + + if (scene_limit.overlap(obj_aabb, overlap_aabb)) { + if (!scene_limit.contain(obj_aabb)) + objs_partially_penetrating_scene_limit.remove(obj); + + hash_table->remove(overlap_aabb, obj); + } else { + objs_outside_scene_limit.remove(obj); + } + + obj_aabb_map.erase(obj); +} + +//============================================================================== +template +void SpatialHashingCollisionManager::setup() { + // Do nothing +} + +//============================================================================== +template +void SpatialHashingCollisionManager::update() { + hash_table->clear(); + objs_partially_penetrating_scene_limit.clear(); + objs_outside_scene_limit.clear(); + + for (auto it = objs.cbegin(), end = objs.cend(); it != end; ++it) { + CollisionObject* obj = *it; + const AABB& obj_aabb = obj->getAABB(); + AABB overlap_aabb; + + if (scene_limit.overlap(obj_aabb, overlap_aabb)) { + if (!scene_limit.contain(obj_aabb)) + objs_partially_penetrating_scene_limit.push_back(obj); + + hash_table->insert(overlap_aabb, obj); + } else { + objs_outside_scene_limit.push_back(obj); + } + + obj_aabb_map[obj] = obj_aabb; + } +} + +//============================================================================== +template +void SpatialHashingCollisionManager::update( + CollisionObject* updated_obj) { + const AABB& new_aabb = updated_obj->getAABB(); + const AABB& old_aabb = obj_aabb_map[updated_obj]; + + AABB old_overlap_aabb; + const auto is_old_aabb_overlapping = + scene_limit.overlap(old_aabb, old_overlap_aabb); + if (is_old_aabb_overlapping) + hash_table->remove(old_overlap_aabb, updated_obj); + + AABB new_overlap_aabb; + const auto is_new_aabb_overlapping = + scene_limit.overlap(new_aabb, new_overlap_aabb); + if (is_new_aabb_overlapping) + hash_table->insert(new_overlap_aabb, updated_obj); + + ObjectStatus old_status; + if (is_old_aabb_overlapping) { + if (scene_limit.contain(old_aabb)) + old_status = Inside; + else + old_status = PartiallyPenetrating; + } else { + old_status = Outside; + } + + if (is_new_aabb_overlapping) { + if (scene_limit.contain(new_aabb)) { + if (old_status == PartiallyPenetrating) { + // Status change: PartiallyPenetrating --> Inside + // Required action(s): + // - remove object from "objs_partially_penetrating_scene_limit" + + auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(), + objs_partially_penetrating_scene_limit.end(), + updated_obj); + objs_partially_penetrating_scene_limit.erase(find_it); + } else if (old_status == Outside) { + // Status change: Outside --> Inside + // Required action(s): + // - remove object from "objs_outside_scene_limit" + + auto find_it = std::find(objs_outside_scene_limit.begin(), + objs_outside_scene_limit.end(), updated_obj); + objs_outside_scene_limit.erase(find_it); + } + } else { + if (old_status == Inside) { + // Status change: Inside --> PartiallyPenetrating + // Required action(s): + // - add object to "objs_partially_penetrating_scene_limit" + + objs_partially_penetrating_scene_limit.push_back(updated_obj); + } else if (old_status == Outside) { + // Status change: Outside --> PartiallyPenetrating + // Required action(s): + // - remove object from "objs_outside_scene_limit" + // - add object to "objs_partially_penetrating_scene_limit" + + auto find_it = std::find(objs_outside_scene_limit.begin(), + objs_outside_scene_limit.end(), updated_obj); + objs_outside_scene_limit.erase(find_it); + + objs_partially_penetrating_scene_limit.push_back(updated_obj); + } + } + } else { + if (old_status == Inside) { + // Status change: Inside --> Outside + // Required action(s): + // - add object to "objs_outside_scene_limit" + + objs_outside_scene_limit.push_back(updated_obj); + } else if (old_status == PartiallyPenetrating) { + // Status change: PartiallyPenetrating --> Outside + // Required action(s): + // - remove object from "objs_partially_penetrating_scene_limit" + // - add object to "objs_outside_scene_limit" + + auto find_it = + std::find(objs_partially_penetrating_scene_limit.begin(), + objs_partially_penetrating_scene_limit.end(), updated_obj); + objs_partially_penetrating_scene_limit.erase(find_it); + + objs_outside_scene_limit.push_back(updated_obj); + } + } + + obj_aabb_map[updated_obj] = new_aabb; +} + +//============================================================================== +template +void SpatialHashingCollisionManager::update( + const std::vector& updated_objs) { + for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]); +} + +//============================================================================== +template +void SpatialHashingCollisionManager::clear() { + objs.clear(); + hash_table->clear(); + objs_outside_scene_limit.clear(); + obj_aabb_map.clear(); +} + +//============================================================================== +template +void SpatialHashingCollisionManager::getObjects( + std::vector& objs_) const { + objs_.resize(objs.size()); + std::copy(objs.begin(), objs.end(), objs_.begin()); +} + +//============================================================================== +template +void SpatialHashingCollisionManager::collide( + CollisionObject* obj, CollisionCallBackBase* callback) const { + if (size() == 0) return; + collide_(obj, callback); +} + +//============================================================================== +template +void SpatialHashingCollisionManager::distance( + CollisionObject* obj, DistanceCallBackBase* callback) const { + if (size() == 0) return; + CoalScalar min_dist = (std::numeric_limits::max)(); + distance_(obj, callback, min_dist); +} + +//============================================================================== +template +bool SpatialHashingCollisionManager::collide_( + CollisionObject* obj, CollisionCallBackBase* callback) const { + const auto& obj_aabb = obj->getAABB(); + AABB overlap_aabb; + + if (scene_limit.overlap(obj_aabb, overlap_aabb)) { + const auto query_result = hash_table->query(overlap_aabb); + for (const auto& obj2 : query_result) { + if (obj == obj2) continue; + + if ((*callback)(obj, obj2)) return true; + } + + if (!scene_limit.contain(obj_aabb)) { + for (const auto& obj2 : objs_outside_scene_limit) { + if (obj == obj2) continue; + + if ((*callback)(obj, obj2)) return true; + } + } + } else { + for (const auto& obj2 : objs_partially_penetrating_scene_limit) { + if (obj == obj2) continue; + + if ((*callback)(obj, obj2)) return true; + } + + for (const auto& obj2 : objs_outside_scene_limit) { + if (obj == obj2) continue; + + if ((*callback)(obj, obj2)) return true; + } + } + + return false; +} + +//============================================================================== +template +bool SpatialHashingCollisionManager::distance_( + CollisionObject* obj, DistanceCallBackBase* callback, + CoalScalar& min_dist) const { + auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; + auto aabb = obj->getAABB(); + if (min_dist < (std::numeric_limits::max)()) { + Vec3s min_dist_delta(min_dist, min_dist, min_dist); + aabb.expand(min_dist_delta); + } + + AABB overlap_aabb; + + auto status = 1; + CoalScalar old_min_distance; + + while (1) { + old_min_distance = min_dist; + + if (scene_limit.overlap(aabb, overlap_aabb)) { + if (distanceObjectToObjects(obj, hash_table->query(overlap_aabb), + callback, min_dist)) { + return true; + } + + if (!scene_limit.contain(aabb)) { + if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback, + min_dist)) { + return true; + } + } + } else { + if (distanceObjectToObjects(obj, objs_partially_penetrating_scene_limit, + callback, min_dist)) { + return true; + } + + if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback, + min_dist)) { + return true; + } + } + + if (status == 1) { + if (old_min_distance < (std::numeric_limits::max)()) { + break; + } else { + if (min_dist < old_min_distance) { + Vec3s min_dist_delta(min_dist, min_dist, min_dist); + aabb = AABB(obj->getAABB(), min_dist_delta); + status = 0; + } else { + if (aabb == obj->getAABB()) + aabb.expand(delta); + else + aabb.expand(obj->getAABB(), 2.0); + } + } + } else if (status == 0) { + break; + } + } + + return false; +} + +//============================================================================== +template +void SpatialHashingCollisionManager::collide( + CollisionCallBackBase* callback) const { + if (size() == 0) return; + + for (const auto& obj1 : objs) { + const auto& obj_aabb = obj1->getAABB(); + AABB overlap_aabb; + + if (scene_limit.overlap(obj_aabb, overlap_aabb)) { + auto query_result = hash_table->query(overlap_aabb); + for (const auto& obj2 : query_result) { + if (obj1 < obj2) { + if ((*callback)(obj1, obj2)) return; + } + } + + if (!scene_limit.contain(obj_aabb)) { + for (const auto& obj2 : objs_outside_scene_limit) { + if (obj1 < obj2) { + if ((*callback)(obj1, obj2)) return; + } + } + } + } else { + for (const auto& obj2 : objs_partially_penetrating_scene_limit) { + if (obj1 < obj2) { + if ((*callback)(obj1, obj2)) return; + } + } + + for (const auto& obj2 : objs_outside_scene_limit) { + if (obj1 < obj2) { + if ((*callback)(obj1, obj2)) return; + } + } + } + } +} + +//============================================================================== +template +void SpatialHashingCollisionManager::distance( + DistanceCallBackBase* callback) const { + if (size() == 0) return; + + this->enable_tested_set_ = true; + this->tested_set.clear(); + + CoalScalar min_dist = (std::numeric_limits::max)(); + + for (const auto& obj : objs) { + if (distance_(obj, callback, min_dist)) break; + } + + this->enable_tested_set_ = false; + this->tested_set.clear(); +} + +//============================================================================== +template +void SpatialHashingCollisionManager::collide( + BroadPhaseCollisionManager* other_manager_, + CollisionCallBackBase* callback) const { + auto* other_manager = + static_cast*>(other_manager_); + + if ((size() == 0) || (other_manager->size() == 0)) return; + + if (this == other_manager) { + collide(callback); + return; + } + + if (this->size() < other_manager->size()) { + for (const auto& obj : objs) { + if (other_manager->collide_(obj, callback)) return; + } + } else { + for (const auto& obj : other_manager->objs) { + if (collide_(obj, callback)) return; + } + } +} + +//============================================================================== +template +void SpatialHashingCollisionManager::distance( + BroadPhaseCollisionManager* other_manager_, + DistanceCallBackBase* callback) const { + auto* other_manager = + static_cast*>(other_manager_); + + if ((size() == 0) || (other_manager->size() == 0)) return; + + if (this == other_manager) { + distance(callback); + return; + } + + CoalScalar min_dist = (std::numeric_limits::max)(); + + if (this->size() < other_manager->size()) { + for (const auto& obj : objs) + if (other_manager->distance_(obj, callback, min_dist)) return; + } else { + for (const auto& obj : other_manager->objs) + if (distance_(obj, callback, min_dist)) return; + } +} + +//============================================================================== +template +bool SpatialHashingCollisionManager::empty() const { + return objs.empty(); +} + +//============================================================================== +template +size_t SpatialHashingCollisionManager::size() const { + return objs.size(); +} + +//============================================================================== +template +void SpatialHashingCollisionManager::computeBound( + std::vector& objs, Vec3s& l, Vec3s& u) { + AABB bound; + for (unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB(); + + l = bound.min_; + u = bound.max_; +} + +//============================================================================== +template +template +bool SpatialHashingCollisionManager::distanceObjectToObjects( + CollisionObject* obj, const Container& objs, DistanceCallBackBase* callback, + CoalScalar& min_dist) const { + for (auto& obj2 : objs) { + if (obj == obj2) continue; + + if (!this->enable_tested_set_) { + if (obj->getAABB().distance(obj2->getAABB()) < min_dist) { + if ((*callback)(obj, obj2, min_dist)) return true; + } + } else { + if (!this->inTestedSet(obj, obj2)) { + if (obj->getAABB().distance(obj2->getAABB()) < min_dist) { + if ((*callback)(obj, obj2, min_dist)) return true; + } + + this->insertTestedSet(obj, obj2); + } + } + } + + return false; +} + +} // namespace coal + +#endif diff --git a/include/coal/broadphase/broadphase_spatialhash.h b/include/coal/broadphase/broadphase_spatialhash.h new file mode 100644 index 000000000..10c4830ce --- /dev/null +++ b/include/coal/broadphase/broadphase_spatialhash.h @@ -0,0 +1,167 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_BROADPAHSESPATIALHASH_H +#define COAL_BROADPHASE_BROADPAHSESPATIALHASH_H + +#include +#include +#include "coal/BV/AABB.h" +#include "coal/broadphase/broadphase_collision_manager.h" +#include "coal/broadphase/detail/simple_hash_table.h" +#include "coal/broadphase/detail/sparse_hash_table.h" +#include "coal/broadphase/detail/spatial_hash.h" + +namespace coal { + +/// @brief spatial hashing collision mananger +template > +class SpatialHashingCollisionManager : public BroadPhaseCollisionManager { + public: + typedef BroadPhaseCollisionManager Base; + using Base::getObjects; + + SpatialHashingCollisionManager(CoalScalar cell_size, const Vec3s& scene_min, + const Vec3s& scene_max, + unsigned int default_table_size = 1000); + + ~SpatialHashingCollisionManager(); + + /// @brief add one object to the manager + void registerObject(CollisionObject* obj); + + /// @brief remove one object from the manager + void unregisterObject(CollisionObject* obj); + + /// @brief initialize the manager, related with the specific type of manager + void setup(); + + /// @brief update the condition of manager + virtual void update(); + + /// @brief update the manager by explicitly given the object updated + void update(CollisionObject* updated_obj); + + /// @brief update the manager by explicitly given the set of objects update + void update(const std::vector& updated_objs); + + /// @brief clear the manager + void clear(); + + /// @brief return the objects managed by the manager + void getObjects(std::vector& objs) const; + + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; + + /// @brief perform distance computation between one object and all the objects + /// belonging ot the manager + void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; + + /// @brief perform collision test for the objects belonging to the manager + /// (i.e, N^2 self collision) + void collide(CollisionCallBackBase* callback) const; + + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + void distance(DistanceCallBackBase* callback) const; + + /// @brief perform collision test with objects belonging to another manager + void collide(BroadPhaseCollisionManager* other_manager, + CollisionCallBackBase* callback) const; + + /// @brief perform distance test with objects belonging to another manager + void distance(BroadPhaseCollisionManager* other_manager, + DistanceCallBackBase* callback) const; + + /// @brief whether the manager is empty + bool empty() const; + + /// @brief the number of objects managed by the manager + size_t size() const; + + /// @brief compute the bound for the environent + static void computeBound(std::vector& objs, Vec3s& l, + Vec3s& u); + + protected: + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; + + /// @brief perform distance computation between one object and all the objects + /// belonging ot the manager + bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, + CoalScalar& min_dist) const; + + /// @brief all objects in the scene + std::list objs; + + /// @brief objects partially penetrating (not totally inside nor outside) the + /// scene limit are in another list + std::list objs_partially_penetrating_scene_limit; + + /// @brief objects outside the scene limit are in another list + std::list objs_outside_scene_limit; + + /// @brief the size of the scene + AABB scene_limit; + + /// @brief store the map between objects and their aabbs. will make update + /// more convenient + std::map obj_aabb_map; + + /// @brief objects in the scene limit (given by scene_min and scene_max) are + /// in the spatial hash table + HashTable* hash_table; + + private: + enum ObjectStatus { Inside, PartiallyPenetrating, Outside }; + + template + bool distanceObjectToObjects(CollisionObject* obj, const Container& objs, + DistanceCallBackBase* callback, + CoalScalar& min_dist) const; +}; + +} // namespace coal + +#include "coal/broadphase/broadphase_spatialhash-inl.h" + +#endif diff --git a/include/coal/broadphase/default_broadphase_callbacks.h b/include/coal/broadphase/default_broadphase_callbacks.h new file mode 100644 index 000000000..711b6c708 --- /dev/null +++ b/include/coal/broadphase/default_broadphase_callbacks.h @@ -0,0 +1,255 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Toyota Research Institute + * Copyright (c) 2022-2023, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Sean Curtis (sean@tri.global) */ +/** @author Justin Carpentier (justin.carpentier@inria.fr) */ + +#ifndef COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H +#define COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H + +#include "coal/broadphase/broadphase_callbacks.h" +#include "coal/collision.h" +#include "coal/distance.h" +// #include "coal/narrowphase/continuous_collision.h" +// #include "coal/narrowphase/continuous_collision_request.h" +// #include "coal/narrowphase/continuous_collision_result.h" +// #include "coal/narrowphase/distance_request.h" +// #include "coal/narrowphase/distance_result.h" + +namespace coal { + +/// @brief Collision data stores the collision request and the result given by +/// collision algorithm. +struct CollisionData { + CollisionData() { done = false; } + + /// @brief Collision request + CollisionRequest request; + + /// @brief Collision result + CollisionResult result; + + /// @brief Whether the collision iteration can stop + bool done; + + /// @brief Clears the CollisionData + void clear() { + result.clear(); + done = false; + } +}; + +/// @brief Distance data stores the distance request and the result given by +/// distance algorithm. +struct DistanceData { + DistanceData() { done = false; } + + /// @brief Distance request + DistanceRequest request; + + /// @brief Distance result + DistanceResult result; + + /// @brief Whether the distance iteration can stop + bool done; + + /// @brief Clears the DistanceData + void clear() { + result.clear(); + done = false; + } +}; + +/// @brief Provides a simple callback for the collision query in the +/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and +/// points to an instance of CollisionData. It simply invokes the +/// `collide()` method on the culled pair of geometries and stores the results +/// in the data's CollisionResult instance. +/// +/// This callback will cause the broadphase evaluation to stop if: +/// - the collision requests _disables_ cost _and_ +/// - the collide() reports a collision for the culled pair, _and_ +/// - we've reported the number of contacts requested in the CollisionRequest. +/// +/// For a given instance of CollisionData, if broadphase evaluation has +/// already terminated (i.e., defaultCollisionFunction() returned `true`), +/// subsequent invocations with the same instance of CollisionData will +/// return immediately, requesting termination of broadphase evaluation (i.e., +/// return `true`). +/// +/// @param o1 The first object in the culled pair. +/// @param o2 The second object in the culled pair. +/// @param data A non-null pointer to a CollisionData instance. +/// @return `true` if the broadphase evaluation should stop. +/// @tparam S The scalar type with which the computation will be performed. +bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, + void* data); + +/// @brief Collision data for use with the DefaultContinuousCollisionFunction. +/// It stores the collision request and the result given by the collision +/// algorithm (and stores the conclusion of whether further evaluation of the +/// broadphase collision manager has been deemed unnecessary). +// struct DefaultContinuousCollisionData { +// ContinuousCollisionRequest request; +// ContinuousCollisionResult result; +// +// /// If `true`, requests that the broadphase evaluation stop. +// bool done{false}; +// }; + +/// @brief Provides a simple callback for the continuous collision query in the +/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and +/// points to an instance of DefaultContinuousCollisionData. It simply invokes +/// the `collide()` method on the culled pair of geometries and stores the +/// results in the data's ContinuousCollisionResult instance. +/// +/// This callback will never cause the broadphase evaluation to terminate early. +/// However, if the `done` member of the DefaultContinuousCollisionData is set +/// to true, this method will simply return without doing any computation. +/// +/// For a given instance of DefaultContinuousCollisionData, if broadphase +/// evaluation has already terminated (i.e., +/// DefaultContinuousCollisionFunction() returned `true`), subsequent +/// invocations with the same instance of CollisionData will return +/// immediately, requesting termination of broadphase evaluation (i.e., return +/// `true`). +/// +/// @param o1 The first object in the culled pair. +/// @param o2 The second object in the culled pair. +/// @param data A non-null pointer to a CollisionData instance. +/// @return True if the broadphase evaluation should stop. +/// @tparam S The scalar type with which the computation will be performed. +// bool DefaultContinuousCollisionFunction(ContinuousCollisionObject* o1, +// ContinuousCollisionObject* o2, +// void* data) { +// assert(data != nullptr); +// auto* cdata = static_cast(data); +// +// if (cdata->done) return true; +// +// const ContinuousCollisionRequest& request = cdata->request; +// ContinuousCollisionResult& result = cdata->result; +// collide(o1, o2, request, result); +// +// return cdata->done; +// } + +/// @brief Provides a simple callback for the distance query in the +/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and +/// points to an instance of DistanceData. It simply invokes the +/// `distance()` method on the culled pair of geometries and stores the results +/// in the data's DistanceResult instance. +/// +/// This callback will cause the broadphase evaluation to stop if: +/// - The distance is less than or equal to zero (i.e., the pair is in +/// contact). +/// +/// For a given instance of DistanceData, if broadphase evaluation has +/// already terminated (i.e., defaultDistanceFunction() returned `true`), +/// subsequent invocations with the same instance of DistanceData will +/// simply report the previously reported minimum distance and request +/// termination of broadphase evaluation (i.e., return `true`). +/// +/// @param o1 The first object in the culled pair. +/// @param o2 The second object in the culled pair. +/// @param data A non-null pointer to a DistanceData instance. +/// @param dist The distance computed by distance(). +/// @return `true` if the broadphase evaluation should stop. +/// @tparam S The scalar type with which the computation will be performed. +bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, + void* data, CoalScalar& dist); + +/// @brief Default collision callback to check collision between collision +/// objects. +struct COAL_DLLAPI CollisionCallBackDefault : CollisionCallBackBase { + /// @brief Initialize the callback. + /// Clears the collision result and sets the done boolean to false. + void init() { data.clear(); } + + bool collide(CollisionObject* o1, CollisionObject* o2); + + CollisionData data; + + virtual ~CollisionCallBackDefault() {}; +}; + +/// @brief Default distance callback to check collision between collision +/// objects. +struct COAL_DLLAPI DistanceCallBackDefault : DistanceCallBackBase { + /// @brief Initialize the callback. + /// Clears the distance result and sets the done boolean to false. + void init() { data.clear(); } + + bool distance(CollisionObject* o1, CollisionObject* o2, CoalScalar& dist); + + DistanceData data; + + virtual ~DistanceCallBackDefault() {}; +}; + +/// @brief Collision callback to collect collision pairs potentially in contacts +struct COAL_DLLAPI CollisionCallBackCollect : CollisionCallBackBase { + typedef std::pair CollisionPair; + + /// @brief Default constructor. + CollisionCallBackCollect(const size_t max_size); + + bool collide(CollisionObject* o1, CollisionObject* o2); + + /// @brief Returns the number of registered collision pairs + size_t numCollisionPairs() const; + + /// @brief Returns a const reference to the active collision_pairs to check + const std::vector& getCollisionPairs() const; + + /// @brief Reset the callback + void init(); + + /// @brief Check whether a collision pair exists + bool exist(const CollisionPair& pair) const; + + /// @brief Check whether a collision pair exists + bool exist(CollisionObject* o1, CollisionObject* o2) const; + + virtual ~CollisionCallBackCollect() {}; + + protected: + std::vector collision_pairs; + size_t max_size; +}; + +} // namespace coal + +#endif // COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H diff --git a/include/coal/broadphase/detail/hierarchy_tree-inl.h b/include/coal/broadphase/detail/hierarchy_tree-inl.h new file mode 100644 index 000000000..af29cf6a5 --- /dev/null +++ b/include/coal/broadphase/detail/hierarchy_tree-inl.h @@ -0,0 +1,998 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_HIERARCHY_TREE_INL_H +#define COAL_HIERARCHY_TREE_INL_H + +#include "coal/broadphase/detail/hierarchy_tree.h" + +namespace coal { + +namespace detail { + +//============================================================================== +template +HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) { + root_node = nullptr; + n_leaves = 0; + free_node = nullptr; + max_lookahead_level = -1; + opath = 0; + bu_threshold = bu_threshold_; + topdown_level = topdown_level_; +} + +//============================================================================== +template +HierarchyTree::~HierarchyTree() { + clear(); +} + +//============================================================================== +template +void HierarchyTree::init(std::vector& leaves, int level) { + switch (level) { + case 0: + init_0(leaves); + break; + case 1: + init_1(leaves); + break; + case 2: + init_2(leaves); + break; + case 3: + init_3(leaves); + break; + default: + init_0(leaves); + } +} + +//============================================================================== +template +typename HierarchyTree::Node* HierarchyTree::insert(const BV& bv, + void* data) { + Node* leaf = createNode(nullptr, bv, data); + insertLeaf(root_node, leaf); + ++n_leaves; + return leaf; +} + +//============================================================================== +template +void HierarchyTree::remove(Node* leaf) { + removeLeaf(leaf); + deleteNode(leaf); + --n_leaves; +} + +//============================================================================== +template +void HierarchyTree::clear() { + if (root_node) recurseDeleteNode(root_node); + n_leaves = 0; + delete free_node; + free_node = nullptr; + max_lookahead_level = -1; + opath = 0; +} + +//============================================================================== +template +bool HierarchyTree::empty() const { + return (nullptr == root_node); +} + +//============================================================================== +template +void HierarchyTree::update(Node* leaf, int lookahead_level) { + // TODO(DamrongGuoy): Since we update a leaf node by removing and + // inserting the same leaf node, it is likely to change the structure of + // the tree even if no object's pose has changed. In the future, + // find a way to preserve the structure of the tree to solve this issue: + // https://github.com/flexible-collision-library/fcl/issues/368 + + // First we remove the leaf node pointed by `leaf` variable from the tree. + // The `sub_root` variable is the root of the subtree containing nodes + // whose BVs were adjusted due to node removal. + typename HierarchyTree::Node* sub_root = removeLeaf(leaf); + if (sub_root) { + if (lookahead_level > 0) { + // For positive `lookahead_level`, we move the `sub_root` variable up. + for (int i = 0; (i < lookahead_level) && sub_root->parent; ++i) + sub_root = sub_root->parent; + } else + // By default, lookahead_level = -1, and we reset the `sub_root` variable + // to the root of the entire tree. + sub_root = root_node; + } + // Then we insert the node pointed by `leaf` variable back into the + // subtree rooted at `sub_root` variable. + insertLeaf(sub_root, leaf); +} + +//============================================================================== +template +bool HierarchyTree::update(Node* leaf, const BV& bv) { + if (leaf->bv.contain(bv)) return false; + update_(leaf, bv); + return true; +} + +//============================================================================== +template +struct UpdateImpl { + static bool run(const HierarchyTree& tree, + typename HierarchyTree::Node* leaf, const BV& bv, + const Vec3s& /*vel*/, CoalScalar /*margin*/) { + if (leaf->bv.contain(bv)) return false; + tree.update_(leaf, bv); + return true; + } + + static bool run(const HierarchyTree& tree, + typename HierarchyTree::Node* leaf, const BV& bv, + const Vec3s& /*vel*/) { + if (leaf->bv.contain(bv)) return false; + tree.update_(leaf, bv); + return true; + } +}; + +//============================================================================== +template +bool HierarchyTree::update(Node* leaf, const BV& bv, const Vec3s& vel, + CoalScalar margin) { + return UpdateImpl::run(*this, leaf, bv, vel, margin); +} + +//============================================================================== +template +bool HierarchyTree::update(Node* leaf, const BV& bv, const Vec3s& vel) { + return UpdateImpl::run(*this, leaf, bv, vel); +} + +//============================================================================== +template +size_t HierarchyTree::getMaxHeight() const { + if (!root_node) return 0; + return getMaxHeight(root_node); +} + +//============================================================================== +template +size_t HierarchyTree::getMaxDepth() const { + if (!root_node) return 0; + + size_t max_depth; + getMaxDepth(root_node, 0, max_depth); + return max_depth; +} + +//============================================================================== +template +void HierarchyTree::balanceBottomup() { + if (root_node) { + std::vector leaves; + leaves.reserve(n_leaves); + fetchLeaves(root_node, leaves); + bottomup(leaves.begin(), leaves.end()); + root_node = leaves[0]; + } +} + +//============================================================================== +template +void HierarchyTree::balanceTopdown() { + if (root_node) { + std::vector leaves; + leaves.reserve(n_leaves); + fetchLeaves(root_node, leaves); + root_node = topdown(leaves.begin(), leaves.end()); + } +} + +//============================================================================== +template +void HierarchyTree::balanceIncremental(int iterations) { + if (iterations < 0) iterations = (int)n_leaves; + if (root_node && (iterations > 0)) { + for (int i = 0; i < iterations; ++i) { + Node* node = root_node; + unsigned int bit = 0; + while (!node->isLeaf()) { + node = sort(node, root_node)->children[(opath >> bit) & 1]; + bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1); + } + update(node); + ++opath; + } + } +} + +//============================================================================== +template +void HierarchyTree::refit() { + if (root_node) recurseRefit(root_node); +} + +//============================================================================== +template +void HierarchyTree::extractLeaves(const Node* root, + std::vector& leaves) const { + if (!root->isLeaf()) { + extractLeaves(root->children[0], leaves); + extractLeaves(root->children[1], leaves); + } else + leaves.push_back(root); +} + +//============================================================================== +template +size_t HierarchyTree::size() const { + return n_leaves; +} + +//============================================================================== +template +typename HierarchyTree::Node* HierarchyTree::getRoot() const { + return root_node; +} + +//============================================================================== +template +typename HierarchyTree::Node*& HierarchyTree::getRoot() { + return root_node; +} + +//============================================================================== +template +void HierarchyTree::print(Node* root, int depth) { + for (int i = 0; i < depth; ++i) std::cout << " "; + std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " + << root->bv.min_[2] << "; " << root->bv.max_[0] << ", " + << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl; + if (root->isLeaf()) { + } else { + print(root->children[0], depth + 1); + print(root->children[1], depth + 1); + } +} + +//============================================================================== +template +void HierarchyTree::bottomup(const NodeVecIterator lbeg, + const NodeVecIterator lend) { + NodeVecIterator lcur_end = lend; + while (lbeg < lcur_end - 1) { + NodeVecIterator min_it1 = lbeg; + NodeVecIterator min_it2 = lbeg + 1; + CoalScalar min_size = (std::numeric_limits::max)(); + for (NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1) { + for (NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2) { + CoalScalar cur_size = ((*it1)->bv + (*it2)->bv).size(); + if (cur_size < min_size) { + min_size = cur_size; + min_it1 = it1; + min_it2 = it2; + } + } + } + + Node* n[2] = {*min_it1, *min_it2}; + Node* p = createNode(nullptr, n[0]->bv, n[1]->bv, nullptr); + p->children[0] = n[0]; + p->children[1] = n[1]; + n[0]->parent = p; + n[1]->parent = p; + *min_it1 = p; + Node* tmp = *min_it2; + lcur_end--; + *min_it2 = *lcur_end; + *lcur_end = tmp; + } +} + +//============================================================================== +template +typename HierarchyTree::Node* HierarchyTree::topdown( + const NodeVecIterator lbeg, const NodeVecIterator lend) { + switch (topdown_level) { + case 0: + return topdown_0(lbeg, lend); + break; + case 1: + return topdown_1(lbeg, lend); + break; + default: + return topdown_0(lbeg, lend); + } +} + +//============================================================================== +template +size_t HierarchyTree::getMaxHeight(Node* node) const { + if (!node->isLeaf()) { + size_t height1 = getMaxHeight(node->children[0]); + size_t height2 = getMaxHeight(node->children[1]); + return std::max(height1, height2) + 1; + } else + return 0; +} + +//============================================================================== +template +void HierarchyTree::getMaxDepth(Node* node, size_t depth, + size_t& max_depth) const { + if (!node->isLeaf()) { + getMaxDepth(node->children[0], depth + 1, max_depth); + getMaxDepth(node->children[1], depth + 1, max_depth); + } else + max_depth = std::max(max_depth, depth); +} + +//============================================================================== +template +typename HierarchyTree::Node* HierarchyTree::topdown_0( + const NodeVecIterator lbeg, const NodeVecIterator lend) { + long num_leaves = lend - lbeg; + if (num_leaves > 1) { + if (num_leaves > bu_threshold) { + BV vol = (*lbeg)->bv; + for (NodeVecIterator it = lbeg + 1; it < lend; ++it) vol += (*it)->bv; + + int best_axis = 0; + CoalScalar extent[3] = {vol.width(), vol.height(), vol.depth()}; + if (extent[1] > extent[0]) best_axis = 1; + if (extent[2] > extent[best_axis]) best_axis = 2; + + // compute median + NodeVecIterator lcenter = lbeg + num_leaves / 2; + std::nth_element(lbeg, lcenter, lend, + std::bind(&nodeBaseLess, std::placeholders::_1, + std::placeholders::_2, std::ref(best_axis))); + + Node* node = createNode(nullptr, vol, nullptr); + node->children[0] = topdown_0(lbeg, lcenter); + node->children[1] = topdown_0(lcenter, lend); + node->children[0]->parent = node; + node->children[1]->parent = node; + return node; + } else { + bottomup(lbeg, lend); + return *lbeg; + } + } + return *lbeg; +} + +//============================================================================== +template +typename HierarchyTree::Node* HierarchyTree::topdown_1( + const NodeVecIterator lbeg, const NodeVecIterator lend) { + long num_leaves = lend - lbeg; + if (num_leaves > 1) { + if (num_leaves > bu_threshold) { + Vec3s split_p = (*lbeg)->bv.center(); + BV vol = (*lbeg)->bv; + NodeVecIterator it; + for (it = lbeg + 1; it < lend; ++it) { + split_p += (*it)->bv.center(); + vol += (*it)->bv; + } + split_p /= static_cast(num_leaves); + int best_axis = -1; + long bestmidp = num_leaves; + int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}}; + for (it = lbeg; it < lend; ++it) { + Vec3s x = (*it)->bv.center() - split_p; + for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0]; + } + + for (int i = 0; i < 3; ++i) { + if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { + long midp = std::abs(splitcount[i][0] - splitcount[i][1]); + if (midp < bestmidp) { + best_axis = i; + bestmidp = midp; + } + } + } + + if (best_axis < 0) best_axis = 0; + + CoalScalar split_value = split_p[best_axis]; + NodeVecIterator lcenter = lbeg; + for (it = lbeg; it < lend; ++it) { + if ((*it)->bv.center()[best_axis] < split_value) { + Node* temp = *it; + *it = *lcenter; + *lcenter = temp; + ++lcenter; + } + } + + Node* node = createNode(nullptr, vol, nullptr); + node->children[0] = topdown_1(lbeg, lcenter); + node->children[1] = topdown_1(lcenter, lend); + node->children[0]->parent = node; + node->children[1]->parent = node; + return node; + } else { + bottomup(lbeg, lend); + return *lbeg; + } + } + return *lbeg; +} + +//============================================================================== +template +void HierarchyTree::init_0(std::vector& leaves) { + clear(); + root_node = topdown(leaves.begin(), leaves.end()); + n_leaves = leaves.size(); + max_lookahead_level = -1; + opath = 0; +} + +//============================================================================== +template +void HierarchyTree::init_1(std::vector& leaves) { + clear(); + + BV bound_bv; + if (leaves.size() > 0) bound_bv = leaves[0]->bv; + for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; + + morton_functor coder(bound_bv); + for (size_t i = 0; i < leaves.size(); ++i) + leaves[i]->code = coder(leaves[i]->bv.center()); + + std::sort(leaves.begin(), leaves.end(), SortByMorton()); + + root_node = mortonRecurse_0(leaves.begin(), leaves.end(), + (1 << (coder.bits() - 1)), coder.bits() - 1); + + refit(); + n_leaves = leaves.size(); + max_lookahead_level = -1; + opath = 0; +} + +//============================================================================== +template +void HierarchyTree::init_2(std::vector& leaves) { + clear(); + + BV bound_bv; + if (leaves.size() > 0) bound_bv = leaves[0]->bv; + for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; + + morton_functor coder(bound_bv); + for (size_t i = 0; i < leaves.size(); ++i) + leaves[i]->code = coder(leaves[i]->bv.center()); + + std::sort(leaves.begin(), leaves.end(), SortByMorton()); + + root_node = mortonRecurse_1(leaves.begin(), leaves.end(), + (1 << (coder.bits() - 1)), coder.bits() - 1); + + refit(); + n_leaves = leaves.size(); + max_lookahead_level = -1; + opath = 0; +} + +//============================================================================== +template +void HierarchyTree::init_3(std::vector& leaves) { + clear(); + + BV bound_bv; + if (leaves.size() > 0) bound_bv = leaves[0]->bv; + for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; + + morton_functor coder(bound_bv); + for (size_t i = 0; i < leaves.size(); ++i) + leaves[i]->code = coder(leaves[i]->bv.center()); + + std::sort(leaves.begin(), leaves.end(), SortByMorton()); + + root_node = mortonRecurse_2(leaves.begin(), leaves.end()); + + refit(); + n_leaves = leaves.size(); + max_lookahead_level = -1; + opath = 0; +} + +//============================================================================== +template +typename HierarchyTree::Node* HierarchyTree::mortonRecurse_0( + const NodeVecIterator lbeg, const NodeVecIterator lend, + const uint32_t& split, int bits) { + long num_leaves = lend - lbeg; + if (num_leaves > 1) { + if (bits > 0) { + Node dummy; + dummy.code = split; + NodeVecIterator lcenter = + std::lower_bound(lbeg, lend, &dummy, SortByMorton()); + + if (lcenter == lbeg) { + uint32_t split2 = split | (1 << (bits - 1)); + return mortonRecurse_0(lbeg, lend, split2, bits - 1); + } else if (lcenter == lend) { + uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); + return mortonRecurse_0(lbeg, lend, split1, bits - 1); + } else { + uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); + uint32_t split2 = split | (1 << (bits - 1)); + + Node* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1); + Node* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1); + Node* node = createNode(nullptr, nullptr); + node->children[0] = child1; + node->children[1] = child2; + child1->parent = node; + child2->parent = node; + return node; + } + } else { + Node* node = topdown(lbeg, lend); + return node; + } + } else + return *lbeg; +} + +//============================================================================== +template +typename HierarchyTree::Node* HierarchyTree::mortonRecurse_1( + const NodeVecIterator lbeg, const NodeVecIterator lend, + const uint32_t& split, int bits) { + long num_leaves = lend - lbeg; + if (num_leaves > 1) { + if (bits > 0) { + Node dummy; + dummy.code = split; + NodeVecIterator lcenter = + std::lower_bound(lbeg, lend, &dummy, SortByMorton()); + + if (lcenter == lbeg) { + uint32_t split2 = split | (1 << (bits - 1)); + return mortonRecurse_1(lbeg, lend, split2, bits - 1); + } else if (lcenter == lend) { + uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); + return mortonRecurse_1(lbeg, lend, split1, bits - 1); + } else { + uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); + uint32_t split2 = split | (1 << (bits - 1)); + + Node* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1); + Node* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1); + Node* node = createNode(nullptr, nullptr); + node->children[0] = child1; + node->children[1] = child2; + child1->parent = node; + child2->parent = node; + return node; + } + } else { + Node* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); + Node* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); + Node* node = createNode(nullptr, nullptr); + node->children[0] = child1; + node->children[1] = child2; + child1->parent = node; + child2->parent = node; + return node; + } + } else + return *lbeg; +} + +//============================================================================== +template +typename HierarchyTree::Node* HierarchyTree::mortonRecurse_2( + const NodeVecIterator lbeg, const NodeVecIterator lend) { + long num_leaves = lend - lbeg; + if (num_leaves > 1) { + Node* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); + Node* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); + Node* node = createNode(nullptr, nullptr); + node->children[0] = child1; + node->children[1] = child2; + child1->parent = node; + child2->parent = node; + return node; + } else + return *lbeg; +} + +//============================================================================== +template +void HierarchyTree::update_(Node* leaf, const BV& bv) { + Node* root = removeLeaf(leaf); + if (root) { + if (max_lookahead_level >= 0) { + for (int i = 0; (i < max_lookahead_level) && root->parent; ++i) + root = root->parent; + } else + root = root_node; + } + + leaf->bv = bv; + insertLeaf(root, leaf); +} + +//============================================================================== +template +typename HierarchyTree::Node* HierarchyTree::sort(Node* n, Node*& r) { + Node* p = n->parent; + if (p > n) { + size_t i = indexOf(n); + size_t j = 1 - i; + Node* s = p->children[j]; + Node* q = p->parent; + if (q) + q->children[indexOf(p)] = n; + else + r = n; + s->parent = n; + p->parent = n; + n->parent = q; + p->children[0] = n->children[0]; + p->children[1] = n->children[1]; + n->children[0]->parent = p; + n->children[1]->parent = p; + n->children[i] = p; + n->children[j] = s; + std::swap(p->bv, n->bv); + return p; + } + return n; +} + +//============================================================================== +template +void HierarchyTree::insertLeaf(Node* const sub_root, Node* const leaf) +// Attempts to insert `leaf` into a subtree rooted at `sub_root`. +// 1. If the whole tree is empty, then `leaf` simply becomes the tree. +// 2. Otherwise, a leaf node called `sibling` of the subtree rooted at +// `sub_root` is selected (the selection criteria is a black box for this +// algorithm), and the `sibling` leaf is replaced by an internal node with +// two children, `sibling` and `leaf`. The bounding volumes are updated as +// necessary. +{ + if (!root_node) { + // If the entire tree is empty, the node pointed by `leaf` variable will + // become the root of the tree. + root_node = leaf; + leaf->parent = nullptr; + return; + } + // Traverse the tree from the given `sub_root` down to an existing leaf + // node that we call `sibling`. The `sibling` node will eventually become + // the sibling of the given `leaf` node. + Node* sibling = sub_root; + while (!sibling->isLeaf()) { + sibling = sibling->children[select(*leaf, *(sibling->children[0]), + *(sibling->children[1]))]; + } + Node* prev = sibling->parent; + // Create a new `node` that later will become the new parent of both the + // `sibling` and the given `leaf`. + Node* node = createNode(prev, leaf->bv, sibling->bv, nullptr); + if (prev) + // If the parent `prev` of the `sibling` is an interior node, we will + // replace the `sibling` with the subtree {node {`sibling`, leaf}} like + // this: + // Before After + // + // ╱ ╱ + // prev prev + // ╱ ╲ ─> ╱ ╲ + // sibling ... node ... + // ╱ ╲ + // sibling leaf + { + prev->children[indexOf(sibling)] = node; + node->children[0] = sibling; + sibling->parent = node; + node->children[1] = leaf; + leaf->parent = node; + // Now that we've inserted `leaf` some of the existing bounding + // volumes might not fully enclose their children. Walk up the tree + // looking for parents that don't already enclose their children + // and create a new tight-fitting bounding volume for those. + do { + if (!prev->bv.contain(node->bv)) + prev->bv = prev->children[0]->bv + prev->children[1]->bv; + else + break; + node = prev; + } while (nullptr != (prev = node->parent)); + } else + // If the `sibling` has no parent, i.e., the tree is a singleton, + // we will replace it with the 3-node tree {node {`sibling`, `leaf`}} like + // this: + // + // node + // ╱ ╲ + // sibling leaf + { + node->children[0] = sibling; + sibling->parent = node; + node->children[1] = leaf; + leaf->parent = node; + root_node = node; + } + + // Note that the above algorithm always adds the new `leaf` node as the right + // child, i.e., children[1]. Calling removeLeaf(l) followed by calling + // this function insertLeaf(l) where l is a left child will result in + // switching l and its sibling even if no object's pose has changed. +} + +//============================================================================== +template +typename HierarchyTree::Node* HierarchyTree::removeLeaf( + Node* const leaf) { + // Deletes `leaf` by replacing the subtree consisting of `leaf`, its sibling, + // and its parent with just its sibling. It then "tightens" the ancestor + // bounding volumes. Returns a pointer to the parent of the highest node whose + // BV changed due to the removal. + if (leaf == root_node) { + // If the `leaf` node is the only node in the tree, the tree becomes empty. + root_node = nullptr; + return nullptr; + } + Node* parent = leaf->parent; + Node* prev = parent->parent; + Node* sibling = parent->children[1 - indexOf(leaf)]; + if (prev) { + // If the parent node is not the root node, the sibling node will + // replace the parent node like this: + // + // Before After + // ... ... + // ╱ ╱ + // prev prev + // ╱ ╲ ╱ ╲ + // parent ... ─> sibling ... + // ╱ ╲ ╱╲ + // leaf sibling ... + // ╱╲ + // ... + // + // Step 1: replace the subtree {parent {leaf, sibling {...}}} with + // {sibling {...}}. + prev->children[indexOf(parent)] = sibling; + sibling->parent = prev; + deleteNode(parent); + // Step 2: tighten up the BVs of the ancestor nodes. + while (prev) { + BV new_bv = prev->children[0]->bv + prev->children[1]->bv; + if (!(new_bv == prev->bv)) { + prev->bv = new_bv; + prev = prev->parent; + } else + break; + } + + return prev ? prev : root_node; + } else { + // If the parent node is the root node, the sibling node will become the + // root of the whole tree like this: + // + // Before After + // + // parent + // ╱ ╲ + // leaf sibling ─> sibling + // ╱╲ ╱╲ + // ... ... + root_node = sibling; + sibling->parent = nullptr; + deleteNode(parent); + return root_node; + } +} + +//============================================================================== +template +void HierarchyTree::fetchLeaves(Node* root, std::vector& leaves, + int depth) { + if ((!root->isLeaf()) && depth) { + fetchLeaves(root->children[0], leaves, depth - 1); + fetchLeaves(root->children[1], leaves, depth - 1); + deleteNode(root); + } else { + leaves.push_back(root); + } +} + +//============================================================================== +template +size_t HierarchyTree::indexOf(Node* node) { + // node cannot be nullptr + return (node->parent->children[1] == node); +} + +//============================================================================== +template +typename HierarchyTree::Node* HierarchyTree::createNode(Node* parent, + const BV& bv, + void* data) { + Node* node = createNode(parent, data); + node->bv = bv; + return node; +} + +//============================================================================== +template +typename HierarchyTree::Node* HierarchyTree::createNode(Node* parent, + const BV& bv1, + const BV& bv2, + void* data) { + Node* node = createNode(parent, data); + node->bv = bv1 + bv2; + return node; +} + +//============================================================================== +template +typename HierarchyTree::Node* HierarchyTree::createNode(Node* parent, + void* data) { + Node* node = nullptr; + if (free_node) { + node = free_node; + free_node = nullptr; + } else + node = new Node(); + node->parent = parent; + node->data = data; + node->children[1] = 0; + return node; +} + +//============================================================================== +template +void HierarchyTree::deleteNode(Node* node) { + if (free_node != node) { + delete free_node; + free_node = node; + } +} + +//============================================================================== +template +void HierarchyTree::recurseDeleteNode(Node* node) { + if (!node->isLeaf()) { + recurseDeleteNode(node->children[0]); + recurseDeleteNode(node->children[1]); + } + + if (node == root_node) root_node = nullptr; + deleteNode(node); +} + +//============================================================================== +template +void HierarchyTree::recurseRefit(Node* node) { + if (!node->isLeaf()) { + recurseRefit(node->children[0]); + recurseRefit(node->children[1]); + node->bv = node->children[0]->bv + node->children[1]->bv; + } else + return; +} + +//============================================================================== +template +bool nodeBaseLess(NodeBase* a, NodeBase* b, int d) { + if (a->bv.center()[d] < b->bv.center()[d]) return true; + return false; +} + +//============================================================================== +template +struct SelectImpl { + static std::size_t run(const NodeBase& /*query*/, + const NodeBase& /*node1*/, + const NodeBase& /*node2*/) { + return 0; + } + + static std::size_t run(const BV& /*query*/, const NodeBase& /*node1*/, + const NodeBase& /*node2*/) { + return 0; + } +}; + +//============================================================================== +template +size_t select(const NodeBase& query, const NodeBase& node1, + const NodeBase& node2) { + return SelectImpl::run(query, node1, node2); +} + +//============================================================================== +template +size_t select(const BV& query, const NodeBase& node1, + const NodeBase& node2) { + return SelectImpl::run(query, node1, node2); +} + +//============================================================================== +template +struct SelectImpl { + static std::size_t run(const NodeBase& node, + const NodeBase& node1, + const NodeBase& node2) { + const AABB& bv = node.bv; + const AABB& bv1 = node1.bv; + const AABB& bv2 = node2.bv; + Vec3s v = bv.min_ + bv.max_; + Vec3s v1 = v - (bv1.min_ + bv1.max_); + Vec3s v2 = v - (bv2.min_ + bv2.max_); + CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); + CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); + return (d1 < d2) ? 0 : 1; + } + + static std::size_t run(const AABB& query, const NodeBase& node1, + const NodeBase& node2) { + const AABB& bv = query; + const AABB& bv1 = node1.bv; + const AABB& bv2 = node2.bv; + Vec3s v = bv.min_ + bv.max_; + Vec3s v1 = v - (bv1.min_ + bv1.max_); + Vec3s v2 = v - (bv2.min_ + bv2.max_); + CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); + CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); + return (d1 < d2) ? 0 : 1; + } +}; + +} // namespace detail +} // namespace coal + +#endif diff --git a/include/coal/broadphase/detail/hierarchy_tree.h b/include/coal/broadphase/detail/hierarchy_tree.h new file mode 100644 index 000000000..6b5fe5261 --- /dev/null +++ b/include/coal/broadphase/detail/hierarchy_tree.h @@ -0,0 +1,292 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_HIERARCHY_TREE_H +#define COAL_HIERARCHY_TREE_H + +#include +#include +#include +#include +#include "coal/warning.hh" +#include "coal/BV/AABB.h" +#include "coal/broadphase/detail/morton.h" +#include "coal/broadphase/detail/node_base.h" + +namespace coal { + +namespace detail { + +/// @brief Class for hierarchy tree structure +template +class HierarchyTree { + public: + typedef NodeBase Node; + + /// @brief Create hierarchy tree with suitable setting. + /// bu_threshold decides the height of tree node to start bottom-up + /// construction / optimization; topdown_level decides different methods to + /// construct tree in topdown manner. lower level method constructs tree with + /// better quality but is slower. + HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0); + + ~HierarchyTree(); + + /// @brief Initialize the tree by a set of leaves using algorithm with a given + /// level. + void init(std::vector& leaves, int level = 0); + + /// @brief Insest a node + Node* insert(const BV& bv, void* data); + + /// @brief Remove a leaf node + void remove(Node* leaf); + + /// @brief Clear the tree + void clear(); + + /// @brief Whether the tree is empty + bool empty() const; + + /// @brief Updates a `leaf` node. A use case is when the bounding volume + /// of an object changes. Ensure every parent node has its bounding volume + /// expand or shrink to fit the bounding volumes of its children. + /// @note Strangely the structure of the tree may change even if the + /// bounding volume of the `leaf` node does not change. It is just + /// another valid tree of the same set of objects. This is because + /// update() works by first removing `leaf` and then inserting `leaf` + /// back. The structural change could be as simple as switching the + /// order of two leaves if the sibling of the `leaf` is also a leaf. + /// Or it could be more complicated if the sibling is an internal + /// node. + void update(Node* leaf, int lookahead_level = -1); + + /// @brief update the tree when the bounding volume of a given leaf has + /// changed + bool update(Node* leaf, const BV& bv); + + /// @brief update one leaf's bounding volume, with prediction + bool update(Node* leaf, const BV& bv, const Vec3s& vel, CoalScalar margin); + + /// @brief update one leaf's bounding volume, with prediction + bool update(Node* leaf, const BV& bv, const Vec3s& vel); + + /// @brief get the max height of the tree + size_t getMaxHeight() const; + + /// @brief get the max depth of the tree + size_t getMaxDepth() const; + + /// @brief balance the tree from bottom + void balanceBottomup(); + + /// @brief balance the tree from top + void balanceTopdown(); + + /// @brief balance the tree in an incremental way + void balanceIncremental(int iterations); + + /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, + /// update the entire tree in a bottom-up manner + void refit(); + + /// @brief extract all the leaves of the tree + void extractLeaves(const Node* root, std::vector& leaves) const; + + /// @brief number of leaves in the tree + size_t size() const; + + /// @brief get the root of the tree + Node* getRoot() const; + + Node*& getRoot(); + + /// @brief print the tree in a recursive way + void print(Node* root, int depth); + + private: + typedef typename std::vector*>::iterator NodeVecIterator; + typedef + typename std::vector*>::const_iterator NodeVecConstIterator; + + struct SortByMorton { + bool operator()(const Node* a, const Node* b) const { + return a->code < b->code; + } + }; + + /// @brief construct a tree for a set of leaves from bottom -- very heavy way + void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend); + + /// @brief construct a tree for a set of leaves from top + Node* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend); + + /// @brief compute the maximum height of a subtree rooted from a given node + size_t getMaxHeight(Node* node) const; + + /// @brief compute the maximum depth of a subtree rooted from a given node + void getMaxDepth(Node* node, size_t depth, size_t& max_depth) const; + + /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a + /// topdown manner. During construction, first compute the best split axis as + /// the axis along with the longest AABB edge. Then compute the median of all + /// nodes' center projection onto the axis and using it as the split + /// threshold. + Node* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend); + + /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a + /// topdown manner. During construction, first compute the best split + /// thresholds for different axes as the average of all nodes' center. Then + /// choose the split axis as the axis whose threshold can divide the nodes + /// into two parts with almost similar size. This construction is more + /// expensive then topdown_0, but also can provide tree with better quality. + Node* topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend); + + /// @brief init tree from leaves in the topdown manner (topdown_0 or + /// topdown_1) + void init_0(std::vector& leaves); + + /// @brief init tree from leaves using morton code. It uses morton_0, i.e., + /// for nodes which is of depth more than the maximum bits of the morton code, + /// we use bottomup method to construct the subtree, which is slow but can + /// construct tree with high quality. + void init_1(std::vector& leaves); + + /// @brief init tree from leaves using morton code. It uses morton_0, i.e., + /// for nodes which is of depth more than the maximum bits of the morton code, + /// we split the leaves into two parts with the same size simply using the + /// node index. + void init_2(std::vector& leaves); + + /// @brief init tree from leaves using morton code. It uses morton_2, i.e., + /// for all nodes, we simply divide the leaves into parts with the same size + /// simply using the node index. + void init_3(std::vector& leaves); + + Node* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, + const uint32_t& split, int bits); + + Node* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, + const uint32_t& split, int bits); + + Node* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend); + + /// @brief update one leaf node's bounding volume + void update_(Node* leaf, const BV& bv); + + /// @brief sort node n and its parent according to their memory position + Node* sort(Node* n, Node*& r); + + /// @brief Insert a leaf node and also update its ancestors. Maintain the + /// tree as a full binary tree (every interior node has exactly two children). + /// Furthermore, adjust the BV of interior nodes so that each parent's BV + /// tightly fits its children's BVs. + /// @param sub_root The root of the subtree into which we will insert the + // leaf node. + void insertLeaf(Node* const sub_root, Node* const leaf); + + /// @brief Remove a leaf. Maintain the tree as a full binary tree (every + /// interior node has exactly two children). Furthermore, adjust the BV of + /// interior nodes so that each parent's BV tightly fits its children's BVs. + /// @note The leaf node itself is not deleted yet, but all the unnecessary + /// internal nodes are deleted. + /// @returns the root of the subtree containing the nodes whose BVs were + // adjusted. + Node* removeLeaf(Node* const leaf); + + /// @brief Delete all internal nodes and return all leaves nodes with given + /// depth from root + void fetchLeaves(Node* root, std::vector& leaves, int depth = -1); + + static size_t indexOf(Node* node); + + /// @brief create one node (leaf or internal) + Node* createNode(Node* parent, const BV& bv, void* data); + + Node* createNode(Node* parent, const BV& bv1, const BV& bv2, void* data); + + Node* createNode(Node* parent, void* data); + + void deleteNode(Node* node); + + void recurseDeleteNode(Node* node); + + void recurseRefit(Node* node); + + protected: + Node* root_node; + + size_t n_leaves; + + unsigned int opath; + + /// This is a one Node cache, the reason is that we need to remove a node and + /// then add it again frequently. + Node* free_node; + + int max_lookahead_level; + + public: + /// @brief decide which topdown algorithm to use + int topdown_level; + + /// @brief decide the depth to use expensive bottom-up algorithm + int bu_threshold; +}; + +/// @brief Compare two nodes accoording to the d-th dimension of node center +template +bool nodeBaseLess(NodeBase* a, NodeBase* b, int d); + +/// @brief select from node1 and node2 which is close to a given query. 0 for +/// node1 and 1 for node2 +template +size_t select(const NodeBase& query, const NodeBase& node1, + const NodeBase& node2); + +/// @brief select from node1 and node2 which is close to a given query bounding +/// volume. 0 for node1 and 1 for node2 +template +size_t select(const BV& query, const NodeBase& node1, + const NodeBase& node2); + +} // namespace detail +} // namespace coal + +#include "coal/broadphase/detail/hierarchy_tree-inl.h" + +#endif diff --git a/include/coal/broadphase/detail/hierarchy_tree_array-inl.h b/include/coal/broadphase/detail/hierarchy_tree_array-inl.h new file mode 100644 index 000000000..fa902e67c --- /dev/null +++ b/include/coal/broadphase/detail/hierarchy_tree_array-inl.h @@ -0,0 +1,978 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_HIERARCHY_TREE_ARRAY_INL_H +#define COAL_HIERARCHY_TREE_ARRAY_INL_H + +#include "coal/broadphase/detail/hierarchy_tree_array.h" + +#include +#include + +namespace coal { + +namespace detail { + +namespace implementation_array { + +//============================================================================== +template +HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) { + root_node = NULL_NODE; + n_nodes = 0; + n_nodes_alloc = 16; + nodes = new Node[n_nodes_alloc]; + for (size_t i = 0; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + n_leaves = 0; + freelist = 0; + opath = 0; + max_lookahead_level = -1; + bu_threshold = bu_threshold_; + topdown_level = topdown_level_; +} + +//============================================================================== +template +HierarchyTree::~HierarchyTree() { + delete[] nodes; +} + +//============================================================================== +template +void HierarchyTree::init(Node* leaves, int n_leaves_, int level) { + switch (level) { + case 0: + init_0(leaves, n_leaves_); + break; + case 1: + init_1(leaves, n_leaves_); + break; + case 2: + init_2(leaves, n_leaves_); + break; + case 3: + init_3(leaves, n_leaves_); + break; + default: + init_0(leaves, n_leaves_); + } +} + +//============================================================================== +template +void HierarchyTree::init_0(Node* leaves, int n_leaves_) { + clear(); + + n_leaves = (size_t)n_leaves_; + root_node = NULL_NODE; + nodes = new Node[n_leaves * 2]; + std::copy(leaves, leaves + n_leaves, nodes); + freelist = n_leaves; + n_nodes = n_leaves; + n_nodes_alloc = 2 * n_leaves; + for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + + size_t* ids = new size_t[n_leaves]; + for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; + + root_node = topdown(ids, ids + n_leaves); + delete[] ids; + + opath = 0; + max_lookahead_level = -1; +} + +//============================================================================== +template +void HierarchyTree::init_1(Node* leaves, int n_leaves_) { + clear(); + + n_leaves = (size_t)n_leaves_; + root_node = NULL_NODE; + nodes = new Node[n_leaves * 2]; + std::copy(leaves, leaves + n_leaves, nodes); + freelist = n_leaves; + n_nodes = n_leaves; + n_nodes_alloc = 2 * n_leaves; + for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + + BV bound_bv; + if (n_leaves > 0) bound_bv = nodes[0].bv; + for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; + + morton_functor coder(bound_bv); + for (size_t i = 0; i < n_leaves; ++i) + nodes[i].code = coder(nodes[i].bv.center()); + + size_t* ids = new size_t[n_leaves]; + for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; + + const SortByMorton comp{nodes}; + std::sort(ids, ids + n_leaves, comp); + root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << (coder.bits() - 1)), + coder.bits() - 1); + delete[] ids; + + refit(); + + opath = 0; + max_lookahead_level = -1; +} + +//============================================================================== +template +void HierarchyTree::init_2(Node* leaves, int n_leaves_) { + clear(); + + n_leaves = (size_t)n_leaves_; + root_node = NULL_NODE; + nodes = new Node[n_leaves * 2]; + std::copy(leaves, leaves + n_leaves, nodes); + freelist = n_leaves; + n_nodes = n_leaves; + n_nodes_alloc = 2 * n_leaves; + for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + + BV bound_bv; + if (n_leaves > 0) bound_bv = nodes[0].bv; + for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; + + morton_functor coder(bound_bv); + for (size_t i = 0; i < n_leaves; ++i) + nodes[i].code = coder(nodes[i].bv.center()); + + size_t* ids = new size_t[n_leaves]; + for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; + + const SortByMorton comp{nodes}; + std::sort(ids, ids + n_leaves, comp); + root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << (coder.bits() - 1)), + coder.bits() - 1); + delete[] ids; + + refit(); + + opath = 0; + max_lookahead_level = -1; +} + +//============================================================================== +template +void HierarchyTree::init_3(Node* leaves, int n_leaves_) { + clear(); + + n_leaves = (size_t)n_leaves_; + root_node = NULL_NODE; + nodes = new Node[n_leaves * 2]; + std::copy(leaves, leaves + n_leaves, nodes); + freelist = n_leaves; + n_nodes = n_leaves; + n_nodes_alloc = 2 * n_leaves; + for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + + BV bound_bv; + if (n_leaves > 0) bound_bv = nodes[0].bv; + for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; + + morton_functor coder(bound_bv); + for (size_t i = 0; i < n_leaves; ++i) + nodes[i].code = coder(nodes[i].bv.center()); + + size_t* ids = new size_t[n_leaves]; + for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; + + const SortByMorton comp{nodes}; + std::sort(ids, ids + n_leaves, comp); + root_node = mortonRecurse_2(ids, ids + n_leaves); + delete[] ids; + + refit(); + + opath = 0; + max_lookahead_level = -1; +} + +//============================================================================== +template +size_t HierarchyTree::insert(const BV& bv, void* data) { + size_t node = createNode(NULL_NODE, bv, data); + insertLeaf(root_node, node); + ++n_leaves; + return node; +} + +//============================================================================== +template +void HierarchyTree::remove(size_t leaf) { + removeLeaf(leaf); + deleteNode(leaf); + --n_leaves; +} + +//============================================================================== +template +void HierarchyTree::clear() { + delete[] nodes; + root_node = NULL_NODE; + n_nodes = 0; + n_nodes_alloc = 16; + nodes = new Node[n_nodes_alloc]; + for (size_t i = 0; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + n_leaves = 0; + freelist = 0; + opath = 0; + max_lookahead_level = -1; +} + +//============================================================================== +template +bool HierarchyTree::empty() const { + return (n_nodes == 0); +} + +//============================================================================== +template +void HierarchyTree::update(size_t leaf, int lookahead_level) { + size_t root = removeLeaf(leaf); + if (root != NULL_NODE) { + if (lookahead_level > 0) { + for (int i = 0; + (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) + root = nodes[root].parent; + } else + root = root_node; + } + insertLeaf(root, leaf); +} + +//============================================================================== +template +bool HierarchyTree::update(size_t leaf, const BV& bv) { + if (nodes[leaf].bv.contain(bv)) return false; + update_(leaf, bv); + return true; +} + +//============================================================================== +template +bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3s& vel, + CoalScalar margin) { + COAL_UNUSED_VARIABLE(bv); + COAL_UNUSED_VARIABLE(vel); + COAL_UNUSED_VARIABLE(margin); + + if (nodes[leaf].bv.contain(bv)) return false; + update_(leaf, bv); + return true; +} + +//============================================================================== +template +bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3s& vel) { + COAL_UNUSED_VARIABLE(vel); + + if (nodes[leaf].bv.contain(bv)) return false; + update_(leaf, bv); + return true; +} + +//============================================================================== +template +size_t HierarchyTree::getMaxHeight() const { + if (root_node == NULL_NODE) return 0; + + return getMaxHeight(root_node); +} + +//============================================================================== +template +size_t HierarchyTree::getMaxDepth() const { + if (root_node == NULL_NODE) return 0; + + size_t max_depth; + getMaxDepth(root_node, 0, max_depth); + return max_depth; +} + +//============================================================================== +template +void HierarchyTree::balanceBottomup() { + if (root_node != NULL_NODE) { + Node* leaves = new Node[n_leaves]; + Node* leaves_ = leaves; + extractLeaves(root_node, leaves_); + root_node = NULL_NODE; + std::copy(leaves, leaves + n_leaves, nodes); + freelist = n_leaves; + n_nodes = n_leaves; + for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + + size_t* ids = new size_t[n_leaves]; + for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; + + bottomup(ids, ids + n_leaves); + root_node = *ids; + + delete[] ids; + } +} + +//============================================================================== +template +void HierarchyTree::balanceTopdown() { + if (root_node != NULL_NODE) { + Node* leaves = new Node[n_leaves]; + Node* leaves_ = leaves; + extractLeaves(root_node, leaves_); + root_node = NULL_NODE; + std::copy(leaves, leaves + n_leaves, nodes); + freelist = n_leaves; + n_nodes = n_leaves; + for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + + size_t* ids = new size_t[n_leaves]; + for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; + + root_node = topdown(ids, ids + n_leaves); + delete[] ids; + } +} + +//============================================================================== +template +void HierarchyTree::balanceIncremental(int iterations) { + if (iterations < 0) iterations = (int)n_leaves; + if ((root_node != NULL_NODE) && (iterations > 0)) { + for (int i = 0; i < iterations; ++i) { + size_t node = root_node; + unsigned int bit = 0; + while (!nodes[node].isLeaf()) { + node = nodes[node].children[(opath >> bit) & 1]; + bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1); + } + update(node); + ++opath; + } + } +} + +//============================================================================== +template +void HierarchyTree::refit() { + if (root_node != NULL_NODE) recurseRefit(root_node); +} + +//============================================================================== +template +void HierarchyTree::extractLeaves(size_t root, Node*& leaves) const { + if (!nodes[root].isLeaf()) { + extractLeaves(nodes[root].children[0], leaves); + extractLeaves(nodes[root].children[1], leaves); + } else { + *leaves = nodes[root]; + leaves++; + } +} + +//============================================================================== +template +size_t HierarchyTree::size() const { + return n_leaves; +} + +//============================================================================== +template +size_t HierarchyTree::getRoot() const { + return root_node; +} + +//============================================================================== +template +typename HierarchyTree::Node* HierarchyTree::getNodes() const { + return nodes; +} + +//============================================================================== +template +void HierarchyTree::print(size_t root, int depth) { + for (int i = 0; i < depth; ++i) std::cout << " "; + Node* n = nodes + root; + std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", " + << n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1] + << ", " << n->bv.max_[2] << ")" << std::endl; + if (n->isLeaf()) { + } else { + print(n->children[0], depth + 1); + print(n->children[1], depth + 1); + } +} + +//============================================================================== +template +size_t HierarchyTree::getMaxHeight(size_t node) const { + if (!nodes[node].isLeaf()) { + size_t height1 = getMaxHeight(nodes[node].children[0]); + size_t height2 = getMaxHeight(nodes[node].children[1]); + return std::max(height1, height2) + 1; + } else + return 0; +} + +//============================================================================== +template +void HierarchyTree::getMaxDepth(size_t node, size_t depth, + size_t& max_depth) const { + if (!nodes[node].isLeaf()) { + getMaxDepth(nodes[node].children[0], depth + 1, max_depth); + getmaxDepth(nodes[node].children[1], depth + 1, max_depth); + } else + max_depth = std::max(max_depth, depth); +} + +//============================================================================== +template +void HierarchyTree::bottomup(size_t* lbeg, size_t* lend) { + size_t* lcur_end = lend; + while (lbeg < lcur_end - 1) { + size_t *min_it1 = nullptr, *min_it2 = nullptr; + CoalScalar min_size = (std::numeric_limits::max)(); + for (size_t* it1 = lbeg; it1 < lcur_end; ++it1) { + for (size_t* it2 = it1 + 1; it2 < lcur_end; ++it2) { + CoalScalar cur_size = (nodes[*it1].bv + nodes[*it2].bv).size(); + if (cur_size < min_size) { + min_size = cur_size; + min_it1 = it1; + min_it2 = it2; + } + } + } + + size_t p = + createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, nullptr); + nodes[p].children[0] = *min_it1; + nodes[p].children[1] = *min_it2; + nodes[*min_it1].parent = p; + nodes[*min_it2].parent = p; + *min_it1 = p; + size_t tmp = *min_it2; + lcur_end--; + *min_it2 = *lcur_end; + *lcur_end = tmp; + } +} + +//============================================================================== +template +size_t HierarchyTree::topdown(size_t* lbeg, size_t* lend) { + switch (topdown_level) { + case 0: + return topdown_0(lbeg, lend); + break; + case 1: + return topdown_1(lbeg, lend); + break; + default: + return topdown_0(lbeg, lend); + } +} + +//============================================================================== +template +size_t HierarchyTree::topdown_0(size_t* lbeg, size_t* lend) { + long num_leaves = lend - lbeg; + if (num_leaves > 1) { + if (num_leaves > bu_threshold) { + BV vol = nodes[*lbeg].bv; + for (size_t* i = lbeg + 1; i < lend; ++i) vol += nodes[*i].bv; + + size_t best_axis = 0; + CoalScalar extent[3] = {vol.width(), vol.height(), vol.depth()}; + if (extent[1] > extent[0]) best_axis = 1; + if (extent[2] > extent[best_axis]) best_axis = 2; + + nodeBaseLess comp(nodes, best_axis); + size_t* lcenter = lbeg + num_leaves / 2; + std::nth_element(lbeg, lcenter, lend, comp); + + size_t node = createNode(NULL_NODE, vol, nullptr); + nodes[node].children[0] = topdown_0(lbeg, lcenter); + nodes[node].children[1] = topdown_0(lcenter, lend); + nodes[nodes[node].children[0]].parent = node; + nodes[nodes[node].children[1]].parent = node; + return node; + } else { + bottomup(lbeg, lend); + return *lbeg; + } + } + return *lbeg; +} + +//============================================================================== +template +size_t HierarchyTree::topdown_1(size_t* lbeg, size_t* lend) { + long num_leaves = lend - lbeg; + if (num_leaves > 1) { + if (num_leaves > bu_threshold) { + Vec3s split_p = nodes[*lbeg].bv.center(); + BV vol = nodes[*lbeg].bv; + for (size_t* i = lbeg + 1; i < lend; ++i) { + split_p += nodes[*i].bv.center(); + vol += nodes[*i].bv; + } + split_p /= static_cast(num_leaves); + int best_axis = -1; + int bestmidp = (int)num_leaves; + int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}}; + for (size_t* i = lbeg; i < lend; ++i) { + Vec3s x = nodes[*i].bv.center() - split_p; + for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0]; + } + + for (size_t i = 0; i < 3; ++i) { + if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { + int midp = std::abs(splitcount[i][0] - splitcount[i][1]); + if (midp < bestmidp) { + best_axis = (int)i; + bestmidp = midp; + } + } + } + + if (best_axis < 0) best_axis = 0; + + CoalScalar split_value = split_p[best_axis]; + size_t* lcenter = lbeg; + for (size_t* i = lbeg; i < lend; ++i) { + if (nodes[*i].bv.center()[best_axis] < split_value) { + size_t temp = *i; + *i = *lcenter; + *lcenter = temp; + ++lcenter; + } + } + + size_t node = createNode(NULL_NODE, vol, nullptr); + nodes[node].children[0] = topdown_1(lbeg, lcenter); + nodes[node].children[1] = topdown_1(lcenter, lend); + nodes[nodes[node].children[0]].parent = node; + nodes[nodes[node].children[1]].parent = node; + return node; + } else { + bottomup(lbeg, lend); + return *lbeg; + } + } + return *lbeg; +} + +//============================================================================== +template +size_t HierarchyTree::mortonRecurse_0(size_t* lbeg, size_t* lend, + const uint32_t& split, int bits) { + long num_leaves = lend - lbeg; + if (num_leaves > 1) { + if (bits > 0) { + const SortByMorton comp{nodes, split}; + size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp); + + if (lcenter == lbeg) { + uint32_t split2 = split | (1 << (bits - 1)); + return mortonRecurse_0(lbeg, lend, split2, bits - 1); + } else if (lcenter == lend) { + uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); + return mortonRecurse_0(lbeg, lend, split1, bits - 1); + } else { + uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); + uint32_t split2 = split | (1 << (bits - 1)); + + size_t child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1); + size_t child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1); + size_t node = createNode(NULL_NODE, nullptr); + nodes[node].children[0] = child1; + nodes[node].children[1] = child2; + nodes[child1].parent = node; + nodes[child2].parent = node; + return node; + } + } else { + size_t node = topdown(lbeg, lend); + return node; + } + } else + return *lbeg; +} + +//============================================================================== +template +size_t HierarchyTree::mortonRecurse_1(size_t* lbeg, size_t* lend, + const uint32_t& split, int bits) { + long num_leaves = lend - lbeg; + if (num_leaves > 1) { + if (bits > 0) { + const SortByMorton comp{nodes, split}; + size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp); + + if (lcenter == lbeg) { + uint32_t split2 = split | (1 << (bits - 1)); + return mortonRecurse_1(lbeg, lend, split2, bits - 1); + } else if (lcenter == lend) { + uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); + return mortonRecurse_1(lbeg, lend, split1, bits - 1); + } else { + uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); + uint32_t split2 = split | (1 << (bits - 1)); + + size_t child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1); + size_t child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1); + size_t node = createNode(NULL_NODE, nullptr); + nodes[node].children[0] = child1; + nodes[node].children[1] = child2; + nodes[child1].parent = node; + nodes[child2].parent = node; + return node; + } + } else { + size_t child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); + size_t child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); + size_t node = createNode(NULL_NODE, nullptr); + nodes[node].children[0] = child1; + nodes[node].children[1] = child2; + nodes[child1].parent = node; + nodes[child2].parent = node; + return node; + } + } else + return *lbeg; +} + +//============================================================================== +template +size_t HierarchyTree::mortonRecurse_2(size_t* lbeg, size_t* lend) { + long num_leaves = lend - lbeg; + if (num_leaves > 1) { + size_t child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); + size_t child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); + size_t node = createNode(NULL_NODE, nullptr); + nodes[node].children[0] = child1; + nodes[node].children[1] = child2; + nodes[child1].parent = node; + nodes[child2].parent = node; + return node; + } else + return *lbeg; +} + +//============================================================================== +template +void HierarchyTree::insertLeaf(size_t root, size_t leaf) { + if (root_node == NULL_NODE) { + root_node = leaf; + nodes[leaf].parent = NULL_NODE; + } else { + if (!nodes[root].isLeaf()) { + do { + root = nodes[root].children[select(leaf, nodes[root].children[0], + nodes[root].children[1], nodes)]; + } while (!nodes[root].isLeaf()); + } + + size_t prev = nodes[root].parent; + size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, nullptr); + if (prev != NULL_NODE) { + nodes[prev].children[indexOf(root)] = node; + nodes[node].children[0] = root; + nodes[root].parent = node; + nodes[node].children[1] = leaf; + nodes[leaf].parent = node; + do { + if (!nodes[prev].bv.contain(nodes[node].bv)) + nodes[prev].bv = nodes[nodes[prev].children[0]].bv + + nodes[nodes[prev].children[1]].bv; + else + break; + node = prev; + } while (NULL_NODE != (prev = nodes[node].parent)); + } else { + nodes[node].children[0] = root; + nodes[root].parent = node; + nodes[node].children[1] = leaf; + nodes[leaf].parent = node; + root_node = node; + } + } +} + +//============================================================================== +template +size_t HierarchyTree::removeLeaf(size_t leaf) { + if (leaf == root_node) { + root_node = NULL_NODE; + return NULL_NODE; + } else { + size_t parent = nodes[leaf].parent; + size_t prev = nodes[parent].parent; + size_t sibling = nodes[parent].children[1 - indexOf(leaf)]; + + if (prev != NULL_NODE) { + nodes[prev].children[indexOf(parent)] = sibling; + nodes[sibling].parent = prev; + deleteNode(parent); + while (prev != NULL_NODE) { + BV new_bv = nodes[nodes[prev].children[0]].bv + + nodes[nodes[prev].children[1]].bv; + if (!(new_bv == nodes[prev].bv)) { + nodes[prev].bv = new_bv; + prev = nodes[prev].parent; + } else + break; + } + + return (prev != NULL_NODE) ? prev : root_node; + } else { + root_node = sibling; + nodes[sibling].parent = NULL_NODE; + deleteNode(parent); + return root_node; + } + } +} + +//============================================================================== +template +void HierarchyTree::update_(size_t leaf, const BV& bv) { + size_t root = removeLeaf(leaf); + if (root != NULL_NODE) { + if (max_lookahead_level >= 0) { + for (int i = 0; + (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) + root = nodes[root].parent; + } + + nodes[leaf].bv = bv; + insertLeaf(root, leaf); + } +} + +//============================================================================== +template +size_t HierarchyTree::indexOf(size_t node) { + return (nodes[nodes[node].parent].children[1] == node); +} + +//============================================================================== +template +size_t HierarchyTree::allocateNode() { + if (freelist == NULL_NODE) { + Node* old_nodes = nodes; + n_nodes_alloc *= 2; + nodes = new Node[n_nodes_alloc]; + std::copy(old_nodes, old_nodes + n_nodes, nodes); + delete[] old_nodes; + + for (size_t i = n_nodes; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + freelist = n_nodes; + } + + size_t node_id = freelist; + freelist = nodes[node_id].next; + nodes[node_id].parent = NULL_NODE; + nodes[node_id].children[0] = NULL_NODE; + nodes[node_id].children[1] = NULL_NODE; + ++n_nodes; + return node_id; +} + +//============================================================================== +template +size_t HierarchyTree::createNode(size_t parent, const BV& bv1, + const BV& bv2, void* data) { + size_t node = allocateNode(); + nodes[node].parent = parent; + nodes[node].data = data; + nodes[node].bv = bv1 + bv2; + return node; +} + +//============================================================================== +template +size_t HierarchyTree::createNode(size_t parent, const BV& bv, void* data) { + size_t node = allocateNode(); + nodes[node].parent = parent; + nodes[node].data = data; + nodes[node].bv = bv; + return node; +} + +//============================================================================== +template +size_t HierarchyTree::createNode(size_t parent, void* data) { + size_t node = allocateNode(); + nodes[node].parent = parent; + nodes[node].data = data; + return node; +} + +//============================================================================== +template +void HierarchyTree::deleteNode(size_t node) { + nodes[node].next = freelist; + freelist = node; + --n_nodes; +} + +//============================================================================== +template +void HierarchyTree::recurseRefit(size_t node) { + if (!nodes[node].isLeaf()) { + recurseRefit(nodes[node].children[0]); + recurseRefit(nodes[node].children[1]); + nodes[node].bv = + nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv; + } else + return; +} + +//============================================================================== +template +void HierarchyTree::fetchLeaves(size_t root, Node*& leaves, int depth) { + if ((!nodes[root].isLeaf()) && depth) { + fetchLeaves(nodes[root].children[0], leaves, depth - 1); + fetchLeaves(nodes[root].children[1], leaves, depth - 1); + deleteNode(root); + } else { + *leaves = nodes[root]; + leaves++; + } +} + +//============================================================================== +template +nodeBaseLess::nodeBaseLess(const NodeBase* nodes_, size_t d_) + : nodes(nodes_), d(d_) { + // Do nothing +} + +//============================================================================== +template +bool nodeBaseLess::operator()(size_t i, size_t j) const { + if (nodes[i].bv.center()[(int)d] < nodes[j].bv.center()[(int)d]) return true; + + return false; +} + +//============================================================================== +template +struct SelectImpl { + static bool run(size_t query, size_t node1, size_t node2, + NodeBase* nodes) { + COAL_UNUSED_VARIABLE(query); + COAL_UNUSED_VARIABLE(node1); + COAL_UNUSED_VARIABLE(node2); + COAL_UNUSED_VARIABLE(nodes); + + return 0; + } + + static bool run(const BV& query, size_t node1, size_t node2, + NodeBase* nodes) { + COAL_UNUSED_VARIABLE(query); + COAL_UNUSED_VARIABLE(node1); + COAL_UNUSED_VARIABLE(node2); + COAL_UNUSED_VARIABLE(nodes); + + return 0; + } +}; + +//============================================================================== +template +size_t select(size_t query, size_t node1, size_t node2, NodeBase* nodes) { + return SelectImpl::run(query, node1, node2, nodes); +} + +//============================================================================== +template +size_t select(const BV& query, size_t node1, size_t node2, + NodeBase* nodes) { + return SelectImpl::run(query, node1, node2, nodes); +} + +//============================================================================== +template +struct SelectImpl { + static bool run(size_t query, size_t node1, size_t node2, + NodeBase* nodes) { + const AABB& bv = nodes[query].bv; + const AABB& bv1 = nodes[node1].bv; + const AABB& bv2 = nodes[node2].bv; + Vec3s v = bv.min_ + bv.max_; + Vec3s v1 = v - (bv1.min_ + bv1.max_); + Vec3s v2 = v - (bv2.min_ + bv2.max_); + CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); + CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); + return (d1 < d2) ? 0 : 1; + } + + static bool run(const AABB& query, size_t node1, size_t node2, + NodeBase* nodes) { + const AABB& bv = query; + const AABB& bv1 = nodes[node1].bv; + const AABB& bv2 = nodes[node2].bv; + Vec3s v = bv.min_ + bv.max_; + Vec3s v1 = v - (bv1.min_ + bv1.max_); + Vec3s v2 = v - (bv2.min_ + bv2.max_); + CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); + CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); + return (d1 < d2) ? 0 : 1; + } +}; + +} // namespace implementation_array +} // namespace detail +} // namespace coal + +#endif diff --git a/include/coal/broadphase/detail/hierarchy_tree_array.h b/include/coal/broadphase/detail/hierarchy_tree_array.h new file mode 100644 index 000000000..e4f12d16f --- /dev/null +++ b/include/coal/broadphase/detail/hierarchy_tree_array.h @@ -0,0 +1,300 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_HIERARCHY_TREE_ARRAY_H +#define COAL_HIERARCHY_TREE_ARRAY_H + +#include +#include +#include + +#include "coal/fwd.hh" +#include "coal/BV/AABB.h" +#include "coal/broadphase/detail/morton.h" +#include "coal/broadphase/detail/node_base_array.h" + +namespace coal { + +namespace detail { + +namespace implementation_array { + +/// @brief Class for hierarchy tree structure +template +class HierarchyTree { + public: + typedef NodeBase Node; + + private: + struct SortByMorton { + SortByMorton(Node* nodes_in) : nodes(nodes_in) {} + SortByMorton(Node* nodes_in, uint32_t split_in) + : nodes(nodes_in), split(split_in) {} + bool operator()(size_t a, size_t b) const { + if ((a != NULL_NODE) && (b != NULL_NODE)) + return nodes[a].code < nodes[b].code; + else if (a == NULL_NODE) + return split < nodes[b].code; + else if (b == NULL_NODE) + return nodes[a].code < split; + + return false; + } + + Node* nodes{}; + uint32_t split{}; + }; + + public: + /// @brief Create hierarchy tree with suitable setting. + /// bu_threshold decides the height of tree node to start bottom-up + /// construction / optimization; topdown_level decides different methods to + /// construct tree in topdown manner. lower level method constructs tree with + /// better quality but is slower. + HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0); + + ~HierarchyTree(); + + /// @brief Initialize the tree by a set of leaves using algorithm with a given + /// level. + void init(Node* leaves, int n_leaves_, int level = 0); + + /// @brief Initialize the tree by a set of leaves using algorithm with a given + /// level. + size_t insert(const BV& bv, void* data); + + /// @brief Remove a leaf node + void remove(size_t leaf); + + /// @brief Clear the tree + void clear(); + + /// @brief Whether the tree is empty + bool empty() const; + + /// @brief update one leaf node + void update(size_t leaf, int lookahead_level = -1); + + /// @brief update the tree when the bounding volume of a given leaf has + /// changed + bool update(size_t leaf, const BV& bv); + + /// @brief update one leaf's bounding volume, with prediction + bool update(size_t leaf, const BV& bv, const Vec3s& vel, CoalScalar margin); + + /// @brief update one leaf's bounding volume, with prediction + bool update(size_t leaf, const BV& bv, const Vec3s& vel); + + /// @brief get the max height of the tree + size_t getMaxHeight() const; + + /// @brief get the max depth of the tree + size_t getMaxDepth() const; + + /// @brief balance the tree from bottom + void balanceBottomup(); + + /// @brief balance the tree from top + void balanceTopdown(); + + /// @brief balance the tree in an incremental way + void balanceIncremental(int iterations); + + /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, + /// update the entire tree in a bottom-up manner + void refit(); + + /// @brief extract all the leaves of the tree + void extractLeaves(size_t root, Node*& leaves) const; + + /// @brief number of leaves in the tree + size_t size() const; + + /// @brief get the root of the tree + size_t getRoot() const; + + /// @brief get the pointer to the nodes array + Node* getNodes() const; + + /// @brief print the tree in a recursive way + void print(size_t root, int depth); + + private: + /// @brief construct a tree for a set of leaves from bottom -- very heavy way + void bottomup(size_t* lbeg, size_t* lend); + + /// @brief construct a tree for a set of leaves from top + size_t topdown(size_t* lbeg, size_t* lend); + + /// @brief compute the maximum height of a subtree rooted from a given node + size_t getMaxHeight(size_t node) const; + + /// @brief compute the maximum depth of a subtree rooted from a given node + void getMaxDepth(size_t node, size_t depth, size_t& max_depth) const; + + /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a + /// topdown manner. During construction, first compute the best split axis as + /// the axis along with the longest AABB edge. Then compute the median of all + /// nodes' center projection onto the axis and using it as the split + /// threshold. + size_t topdown_0(size_t* lbeg, size_t* lend); + + /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a + /// topdown manner. During construction, first compute the best split + /// thresholds for different axes as the average of all nodes' center. Then + /// choose the split axis as the axis whose threshold can divide the nodes + /// into two parts with almost similar size. This construction is more + /// expensive then topdown_0, but also can provide tree with better quality. + size_t topdown_1(size_t* lbeg, size_t* lend); + + /// @brief init tree from leaves in the topdown manner (topdown_0 or + /// topdown_1) + void init_0(Node* leaves, int n_leaves_); + + /// @brief init tree from leaves using morton code. It uses morton_0, i.e., + /// for nodes which is of depth more than the maximum bits of the morton code, + /// we use bottomup method to construct the subtree, which is slow but can + /// construct tree with high quality. + void init_1(Node* leaves, int n_leaves_); + + /// @brief init tree from leaves using morton code. It uses morton_0, i.e., + /// for nodes which is of depth more than the maximum bits of the morton code, + /// we split the leaves into two parts with the same size simply using the + /// node index. + void init_2(Node* leaves, int n_leaves_); + + /// @brief init tree from leaves using morton code. It uses morton_2, i.e., + /// for all nodes, we simply divide the leaves into parts with the same size + /// simply using the node index. + void init_3(Node* leaves, int n_leaves_); + + size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const uint32_t& split, + int bits); + + size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const uint32_t& split, + int bits); + + size_t mortonRecurse_2(size_t* lbeg, size_t* lend); + + /// @brief update one leaf node's bounding volume + void update_(size_t leaf, const BV& bv); + + /// @brief Insert a leaf node and also update its ancestors + void insertLeaf(size_t root, size_t leaf); + + /// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the + /// unnecessary internal nodes are deleted. return the node with the smallest + /// depth and is influenced by the remove operation + size_t removeLeaf(size_t leaf); + + /// @brief Delete all internal nodes and return all leaves nodes with given + /// depth from root + void fetchLeaves(size_t root, Node*& leaves, int depth = -1); + + size_t indexOf(size_t node); + + size_t allocateNode(); + + /// @brief create one node (leaf or internal) + size_t createNode(size_t parent, const BV& bv1, const BV& bv2, void* data); + + size_t createNode(size_t parent, const BV& bv, void* data); + + size_t createNode(size_t parent, void* data); + + void deleteNode(size_t node); + + void recurseRefit(size_t node); + + protected: + size_t root_node; + Node* nodes; + size_t n_nodes; + size_t n_nodes_alloc; + + size_t n_leaves; + size_t freelist; + unsigned int opath; + + int max_lookahead_level; + + public: + /// @brief decide which topdown algorithm to use + int topdown_level; + + /// @brief decide the depth to use expensive bottom-up algorithm + int bu_threshold; + + public: + static const size_t NULL_NODE = std::numeric_limits::max(); +}; + +template +const size_t HierarchyTree::NULL_NODE; + +/// @brief Functor comparing two nodes +template +struct nodeBaseLess { + nodeBaseLess(const NodeBase* nodes_, size_t d_); + + bool operator()(size_t i, size_t j) const; + + private: + /// @brief the nodes array + const NodeBase* nodes; + + /// @brief the dimension to compare + size_t d; +}; + +/// @brief select the node from node1 and node2 which is close to the query-th +/// node in the nodes. 0 for node1 and 1 for node2. +template +size_t select(size_t query, size_t node1, size_t node2, NodeBase* nodes); + +/// @brief select the node from node1 and node2 which is close to the query +/// AABB. 0 for node1 and 1 for node2. +template +size_t select(const BV& query, size_t node1, size_t node2, NodeBase* nodes); + +} // namespace implementation_array +} // namespace detail +} // namespace coal + +#include "coal/broadphase/detail/hierarchy_tree_array-inl.h" + +#endif diff --git a/include/coal/broadphase/detail/interval_tree.h b/include/coal/broadphase/detail/interval_tree.h new file mode 100644 index 000000000..e2f8485c6 --- /dev/null +++ b/include/coal/broadphase/detail/interval_tree.h @@ -0,0 +1,127 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_INTERVAL_TREE_H +#define COAL_INTERVAL_TREE_H + +#include +#include +#include + +#include "coal/broadphase/detail/interval_tree_node.h" + +namespace coal { +namespace detail { + +/// @brief Class describes the information needed when we take the +/// right branch in searching for intervals but possibly come back +/// and check the left branch as well. +struct COAL_DLLAPI it_recursion_node { + public: + IntervalTreeNode* start_node; + + unsigned int parent_index; + + bool try_right_branch; +}; + +/// @brief Interval tree +class COAL_DLLAPI IntervalTree { + public: + IntervalTree(); + + ~IntervalTree(); + + /// @brief Print the whole interval tree + void print() const; + + /// @brief Delete one node of the interval tree + SimpleInterval* deleteNode(IntervalTreeNode* node); + + /// @brief delete node stored a given interval + void deleteNode(SimpleInterval* ivl); + + /// @brief Insert one node of the interval tree + IntervalTreeNode* insert(SimpleInterval* new_interval); + + /// @brief get the predecessor of a given node + IntervalTreeNode* getPredecessor(IntervalTreeNode* node) const; + + /// @brief Get the successor of a given node + IntervalTreeNode* getSuccessor(IntervalTreeNode* node) const; + + /// @brief Return result for a given query + std::deque query(CoalScalar low, CoalScalar high); + + protected: + IntervalTreeNode* root; + + IntervalTreeNode* invalid_node; + + /// @brief left rotation of tree node + void leftRotate(IntervalTreeNode* node); + + /// @brief right rotation of tree node + void rightRotate(IntervalTreeNode* node); + + /// @brief Inserts node into the tree as if it were a regular binary tree + void recursiveInsert(IntervalTreeNode* node); + + /// @brief recursively print a subtree + void recursivePrint(IntervalTreeNode* node) const; + + /// @brief recursively find the node corresponding to the interval + IntervalTreeNode* recursiveSearch(IntervalTreeNode* node, + SimpleInterval* ivl) const; + + /// @brief Travels up to the root fixing the max_high fields after an + /// insertion or deletion + void fixupMaxHigh(IntervalTreeNode* node); + + void deleteFixup(IntervalTreeNode* node); + + private: + unsigned int recursion_node_stack_size; + it_recursion_node* recursion_node_stack; + unsigned int current_parent; + unsigned int recursion_node_stack_top; +}; + +} // namespace detail +} // namespace coal + +#endif diff --git a/include/hpp/fcl/broadphase/detail/interval_tree_node-inl.h b/include/coal/broadphase/detail/interval_tree_node-inl.h similarity index 92% rename from include/hpp/fcl/broadphase/detail/interval_tree_node-inl.h rename to include/coal/broadphase/detail/interval_tree_node-inl.h index b9771ae18..1edd7e6d2 100644 --- a/include/hpp/fcl/broadphase/detail/interval_tree_node-inl.h +++ b/include/coal/broadphase/detail/interval_tree_node-inl.h @@ -35,16 +35,15 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H -#define HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H +#ifndef COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H +#define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H -#include "hpp/fcl/broadphase/detail/interval_tree_node.h" +#include "coal/broadphase/detail/interval_tree_node.h" #include #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { //============================================================================== @@ -90,7 +89,6 @@ void IntervalTreeNode::print(IntervalTreeNode* nil, } } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/coal/broadphase/detail/interval_tree_node.h b/include/coal/broadphase/detail/interval_tree_node.h new file mode 100644 index 000000000..01c5de4bd --- /dev/null +++ b/include/coal/broadphase/detail/interval_tree_node.h @@ -0,0 +1,90 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_DETAIL_INTERVALTREENODE_H +#define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_H + +#include "coal/broadphase/detail/simple_interval.h" +#include "coal/fwd.hh" + +namespace coal { + +namespace detail { + +class COAL_DLLAPI IntervalTree; + +/// @brief The node for interval tree +class COAL_DLLAPI IntervalTreeNode { + public: + friend class IntervalTree; + + /// @brief Create an empty node + IntervalTreeNode(); + + /// @brief Create an node storing the interval + IntervalTreeNode(SimpleInterval* new_interval); + + ~IntervalTreeNode(); + + /// @brief Print the interval node information: set left = invalid_node and + /// right = root + void print(IntervalTreeNode* left, IntervalTreeNode* right) const; + + protected: + /// @brief interval stored in the node + SimpleInterval* stored_interval; + + CoalScalar key; + + CoalScalar high; + + CoalScalar max_high; + + /// @brief red or black node: if red = false then the node is black + bool red; + + IntervalTreeNode* left; + + IntervalTreeNode* right; + + IntervalTreeNode* parent; +}; + +} // namespace detail +} // namespace coal + +#endif diff --git a/include/coal/broadphase/detail/morton-inl.h b/include/coal/broadphase/detail/morton-inl.h new file mode 100644 index 000000000..b646f299e --- /dev/null +++ b/include/coal/broadphase/detail/morton-inl.h @@ -0,0 +1,151 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * Copyright (c) 2016, Toyota Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_MORTON_INL_H +#define COAL_MORTON_INL_H + +#include "coal/broadphase/detail/morton.h" + +namespace coal { /// @cond IGNORE +namespace detail { + +//============================================================================== +template +uint32_t quantize(S x, uint32_t n) { + return std::max(std::min((uint32_t)(x * (S)n), uint32_t(n - 1)), uint32_t(0)); +} + +//============================================================================== +template +morton_functor::morton_functor(const AABB& bbox) + : base(bbox.min_), + inv(1.0 / (bbox.max_[0] - bbox.min_[0]), + 1.0 / (bbox.max_[1] - bbox.min_[1]), + 1.0 / (bbox.max_[2] - bbox.min_[2])) { + // Do nothing +} + +//============================================================================== +template +uint32_t morton_functor::operator()(const Vec3s& point) const { + uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u); + uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u); + uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u); + + return detail::morton_code(x, y, z); +} + +//============================================================================== +template +morton_functor::morton_functor(const AABB& bbox) + : base(bbox.min_), + inv(1.0 / (bbox.max_[0] - bbox.min_[0]), + 1.0 / (bbox.max_[1] - bbox.min_[1]), + 1.0 / (bbox.max_[2] - bbox.min_[2])) { + // Do nothing +} + +//============================================================================== +template +uint64_t morton_functor::operator()(const Vec3s& point) const { + uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20); + uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20); + uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20); + + return detail::morton_code60(x, y, z); +} + +//============================================================================== +template +constexpr size_t morton_functor::bits() { + return 60; +} + +//============================================================================== +template +constexpr size_t morton_functor::bits() { + return 30; +} + +//============================================================================== +template +morton_functor>::morton_functor(const AABB& bbox) + : base(bbox.min_), + inv(1.0 / (bbox.max_[0] - bbox.min_[0]), + 1.0 / (bbox.max_[1] - bbox.min_[1]), + 1.0 / (bbox.max_[2] - bbox.min_[2])) { + // Do nothing +} + +//============================================================================== +template +std::bitset morton_functor>::operator()( + const Vec3s& point) const { + S x = (point[0] - base[0]) * inv[0]; + S y = (point[1] - base[1]) * inv[1]; + S z = (point[2] - base[2]) * inv[2]; + int start_bit = bits() - 1; + std::bitset bset; + + x *= 2; + y *= 2; + z *= 2; + + for (size_t i = 0; i < bits() / 3; ++i) { + bset[start_bit--] = ((z < 1) ? 0 : 1); + bset[start_bit--] = ((y < 1) ? 0 : 1); + bset[start_bit--] = ((x < 1) ? 0 : 1); + x = ((x >= 1) ? 2 * (x - 1) : 2 * x); + y = ((y >= 1) ? 2 * (y - 1) : 2 * y); + z = ((z >= 1) ? 2 * (z - 1) : 2 * z); + } + + return bset; +} + +//============================================================================== +template +constexpr size_t morton_functor>::bits() { + return N; +} + +} // namespace detail +/// @endcond +} // namespace coal + +#endif diff --git a/include/coal/broadphase/detail/morton.h b/include/coal/broadphase/detail/morton.h new file mode 100644 index 000000000..f7a9c9473 --- /dev/null +++ b/include/coal/broadphase/detail/morton.h @@ -0,0 +1,120 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * Copyright (c) 2016, Toyota Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_MORTON_H +#define COAL_MORTON_H + +#include "coal/BV/AABB.h" +#include +#include + +namespace coal { + +/// @cond IGNORE +namespace detail { + +template +uint32_t quantize(S x, uint32_t n); + +/// @brief compute 30 bit morton code +COAL_DLLAPI +uint32_t morton_code(uint32_t x, uint32_t y, uint32_t z); + +/// @brief compute 60 bit morton code +COAL_DLLAPI +uint64_t morton_code60(uint32_t x, uint32_t y, uint32_t z); + +/// @brief Functor to compute the morton code for a given AABB +/// This is specialized for 32- and 64-bit unsigned integers giving +/// a 30- or 60-bit code, respectively, and for `std::bitset` where +/// N is the length of the code and must be a multiple of 3. +template +struct morton_functor {}; + +/// @brief Functor to compute 30 bit morton code for a given AABB +template +struct morton_functor { + morton_functor(const AABB& bbox); + + uint32_t operator()(const Vec3s& point) const; + + const Vec3s base; + const Vec3s inv; + + static constexpr size_t bits(); +}; + +using morton_functoru32f = morton_functor; +using morton_functoru32d = morton_functor; + +/// @brief Functor to compute 60 bit morton code for a given AABB +template +struct morton_functor { + morton_functor(const AABB& bbox); + + uint64_t operator()(const Vec3s& point) const; + + const Vec3s base; + const Vec3s inv; + + static constexpr size_t bits(); +}; + +/// @brief Functor to compute N bit morton code for a given AABB +/// N must be a multiple of 3. +template +struct morton_functor> { + static_assert(N % 3 == 0, "Number of bits must be a multiple of 3"); + + morton_functor(const AABB& bbox); + + std::bitset operator()(const Vec3s& point) const; + + const Vec3s base; + const Vec3s inv; + + static constexpr size_t bits(); +}; + +} // namespace detail +/// @endcond +} // namespace coal + +#include "coal/broadphase/detail/morton-inl.h" + +#endif diff --git a/include/coal/broadphase/detail/node_base-inl.h b/include/coal/broadphase/detail/node_base-inl.h new file mode 100644 index 000000000..db63bcee1 --- /dev/null +++ b/include/coal/broadphase/detail/node_base-inl.h @@ -0,0 +1,75 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_DETAIL_NODEBASE_INL_H +#define COAL_BROADPHASE_DETAIL_NODEBASE_INL_H + +#include "coal/broadphase/detail/node_base.h" + +namespace coal { +namespace detail { + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +bool NodeBase::isLeaf() const { + return (children[1] == nullptr); +} + +//============================================================================== +template +bool NodeBase::isInternal() const { + return !isLeaf(); +} + +//============================================================================== +template +NodeBase::NodeBase() { + parent = nullptr; + children[0] = nullptr; + children[1] = nullptr; +} + +} // namespace detail +} // namespace coal + +#endif diff --git a/include/coal/broadphase/detail/node_base.h b/include/coal/broadphase/detail/node_base.h new file mode 100644 index 000000000..179495e6f --- /dev/null +++ b/include/coal/broadphase/detail/node_base.h @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_DETAIL_NODEBASE_H +#define COAL_BROADPHASE_DETAIL_NODEBASE_H + +#include "coal/data_types.h" + +namespace coal { + +namespace detail { + +/// @brief dynamic AABB tree node +template +struct NodeBase { + /// @brief the bounding volume for the node + BV bv; + + /// @brief pointer to parent node + NodeBase* parent; + + /// @brief whether is a leaf + bool isLeaf() const; + + /// @brief whether is internal node + bool isInternal() const; + + union { + /// @brief for leaf node, children nodes + NodeBase* children[2]; + void* data; + }; + + /// @brief morton code for current BV + uint32_t code; + + NodeBase(); +}; + +} // namespace detail +} // namespace coal + +#include "coal/broadphase/detail/node_base-inl.h" + +#endif diff --git a/include/coal/broadphase/detail/node_base_array-inl.h b/include/coal/broadphase/detail/node_base_array-inl.h new file mode 100644 index 000000000..a48fceef9 --- /dev/null +++ b/include/coal/broadphase/detail/node_base_array-inl.h @@ -0,0 +1,65 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H +#define COAL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H + +#include "coal/broadphase/detail/node_base_array.h" + +namespace coal { + +namespace detail { + +namespace implementation_array { + +//============================================================================== +template +bool NodeBase::isLeaf() const { + return (children[1] == (size_t)(-1)); +} + +//============================================================================== +template +bool NodeBase::isInternal() const { + return !isLeaf(); +} + +} // namespace implementation_array +} // namespace detail +} // namespace coal + +#endif diff --git a/include/coal/broadphase/detail/node_base_array.h b/include/coal/broadphase/detail/node_base_array.h new file mode 100644 index 000000000..5c18ef1f7 --- /dev/null +++ b/include/coal/broadphase/detail/node_base_array.h @@ -0,0 +1,75 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_DETAIL_NODEBASEARRAY_H +#define COAL_BROADPHASE_DETAIL_NODEBASEARRAY_H + +#include "coal/data_types.h" + +namespace coal { + +namespace detail { + +namespace implementation_array { + +template +struct NodeBase { + BV bv; + + union { + size_t parent; + size_t next; + }; + + union { + size_t children[2]; + void* data; + }; + + uint32_t code; + + bool isLeaf() const; + bool isInternal() const; +}; + +} // namespace implementation_array +} // namespace detail +} // namespace coal + +#include "coal/broadphase/detail/node_base_array-inl.h" + +#endif diff --git a/include/coal/broadphase/detail/simple_hash_table-inl.h b/include/coal/broadphase/detail/simple_hash_table-inl.h new file mode 100644 index 000000000..ae534f037 --- /dev/null +++ b/include/coal/broadphase/detail/simple_hash_table-inl.h @@ -0,0 +1,111 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_SIMPLEHASHTABLE_INL_H +#define COAL_BROADPHASE_SIMPLEHASHTABLE_INL_H + +#include "coal/broadphase/detail/simple_hash_table.h" + +#include +namespace coal { + +namespace detail { + +//============================================================================== +template +SimpleHashTable::SimpleHashTable(const HashFnc& h) : h_(h) { + // Do nothing +} + +//============================================================================== +template +void SimpleHashTable::init(size_t size) { + if (size == 0) { + COAL_THROW_PRETTY("SimpleHashTable must have non-zero size.", + std::logic_error); + } + + table_.resize(size); + table_size_ = size; +} + +//============================================================================== +template +void SimpleHashTable::insert(Key key, Data value) { + std::vector indices = h_(key); + size_t range = table_.size(); + for (size_t i = 0; i < indices.size(); ++i) + table_[indices[i] % range].push_back(value); +} + +//============================================================================== +template +std::vector SimpleHashTable::query(Key key) const { + size_t range = table_.size(); + std::vector indices = h_(key); + std::set result; + for (size_t i = 0; i < indices.size(); ++i) { + size_t index = indices[i] % range; + std::copy(table_[index].begin(), table_[index].end(), + std::inserter(result, result.end())); + } + + return std::vector(result.begin(), result.end()); +} + +//============================================================================== +template +void SimpleHashTable::remove(Key key, Data value) { + size_t range = table_.size(); + std::vector indices = h_(key); + for (size_t i = 0; i < indices.size(); ++i) { + size_t index = indices[i] % range; + table_[index].remove(value); + } +} + +//============================================================================== +template +void SimpleHashTable::clear() { + table_.clear(); + table_.resize(table_size_); +} + +} // namespace detail +} // namespace coal + +#endif diff --git a/include/coal/broadphase/detail/simple_hash_table.h b/include/coal/broadphase/detail/simple_hash_table.h new file mode 100644 index 000000000..1b4749026 --- /dev/null +++ b/include/coal/broadphase/detail/simple_hash_table.h @@ -0,0 +1,87 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_SIMPLEHASHTABLE_H +#define COAL_BROADPHASE_SIMPLEHASHTABLE_H + +#include +#include +#include + +namespace coal { + +namespace detail { + +/// @brief A simple hash table implemented as multiple buckets. HashFnc is any +/// extended hash function: HashFnc(key) = {index1, index2, ..., } +template +class SimpleHashTable { + protected: + typedef std::list Bin; + + std::vector table_; + + HashFnc h_; + + size_t table_size_; + + public: + SimpleHashTable(const HashFnc& h); + + /// @brief Init the number of bins in the hash table + void init(size_t size); + + //// @brief Insert a key-value pair into the table + void insert(Key key, Data value); + + /// @brief Find the elements in the hash table whose key is the same as query + /// key. + std::vector query(Key key) const; + + /// @brief remove the key-value pair from the table + void remove(Key key, Data value); + + /// @brief clear the hash table + void clear(); +}; + +} // namespace detail +} // namespace coal + +#include "coal/broadphase/detail/simple_hash_table-inl.h" + +#endif diff --git a/include/coal/broadphase/detail/simple_interval-inl.h b/include/coal/broadphase/detail/simple_interval-inl.h new file mode 100644 index 000000000..95075cbc1 --- /dev/null +++ b/include/coal/broadphase/detail/simple_interval-inl.h @@ -0,0 +1,60 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H +#define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H + +#include "coal/broadphase/detail/simple_interval.h" +#include + +namespace coal { +namespace detail { + +//============================================================================== +SimpleInterval::~SimpleInterval() { + // Do nothing +} + +//============================================================================== +void SimpleInterval::print() { + // Do nothing +} + +} // namespace detail +} // namespace coal + +#endif diff --git a/include/coal/broadphase/detail/simple_interval.h b/include/coal/broadphase/detail/simple_interval.h new file mode 100644 index 000000000..efd9cbe55 --- /dev/null +++ b/include/coal/broadphase/detail/simple_interval.h @@ -0,0 +1,62 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H +#define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H + +#include "coal/fwd.hh" +#include "coal/data_types.h" + +namespace coal { +namespace detail { + +/// @brief Interval trees implemented using red-black-trees as described in +/// the book Introduction_To_Algorithms_ by Cormen, Leisserson, and Rivest. +struct COAL_DLLAPI SimpleInterval { + public: + virtual ~SimpleInterval(); + + virtual void print(); + + /// @brief interval is defined as [low, high] + CoalScalar low, high; +}; + +} // namespace detail +} // namespace coal + +#endif diff --git a/include/coal/broadphase/detail/sparse_hash_table-inl.h b/include/coal/broadphase/detail/sparse_hash_table-inl.h new file mode 100644 index 000000000..299f9871a --- /dev/null +++ b/include/coal/broadphase/detail/sparse_hash_table-inl.h @@ -0,0 +1,111 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_SPARSEHASHTABLE_INL_H +#define COAL_BROADPHASE_SPARSEHASHTABLE_INL_H + +#include "coal/broadphase/detail/sparse_hash_table.h" + +namespace coal { + +namespace detail { + +//============================================================================== +template class TableT> +SparseHashTable::SparseHashTable(const HashFnc& h) + : h_(h) { + // Do nothing +} + +//============================================================================== +template class TableT> +void SparseHashTable::init(size_t) { + table_.clear(); +} + +//============================================================================== +template class TableT> +void SparseHashTable::insert(Key key, Data value) { + std::vector indices = h_(key); + for (size_t i = 0; i < indices.size(); ++i) + table_[indices[i]].push_back(value); +} + +//============================================================================== +template class TableT> +std::vector SparseHashTable::query( + Key key) const { + std::vector indices = h_(key); + std::set result; + for (size_t i = 0; i < indices.size(); ++i) { + unsigned int index = indices[i]; + typename Table::const_iterator p = table_.find(index); + if (p != table_.end()) { + std::copy((*p).second.begin(), (*p).second.end(), + std ::inserter(result, result.end())); + } + } + + return std::vector(result.begin(), result.end()); +} + +//============================================================================== +template class TableT> +void SparseHashTable::remove(Key key, Data value) { + std::vector indices = h_(key); + for (size_t i = 0; i < indices.size(); ++i) { + unsigned int index = indices[i]; + table_[index].remove(value); + } +} + +//============================================================================== +template class TableT> +void SparseHashTable::clear() { + table_.clear(); +} + +} // namespace detail +} // namespace coal + +#endif diff --git a/include/coal/broadphase/detail/sparse_hash_table.h b/include/coal/broadphase/detail/sparse_hash_table.h new file mode 100644 index 000000000..13c4aeeca --- /dev/null +++ b/include/coal/broadphase/detail/sparse_hash_table.h @@ -0,0 +1,93 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_SPARSEHASHTABLE_H +#define COAL_BROADPHASE_SPARSEHASHTABLE_H + +#include +#include +#include +#include + +namespace coal { + +namespace detail { + +template +class unordered_map_hash_table : public std::unordered_map { + typedef std::unordered_map Base; + + public: + unordered_map_hash_table() : Base() {}; +}; + +/// @brief A hash table implemented using unordered_map +template class TableT = unordered_map_hash_table> +class SparseHashTable { + protected: + HashFnc h_; + typedef std::list Bin; + typedef TableT Table; + + Table table_; + + public: + SparseHashTable(const HashFnc& h); + + /// @brief Init the hash table. The bucket size is dynamically decided. + void init(size_t); + + /// @brief insert one key-value pair into the hash table + void insert(Key key, Data value); + + /// @brief find the elements whose key is the same as the query + std::vector query(Key key) const; + + /// @brief remove one key-value pair from the hash table + void remove(Key key, Data value); + + /// @brief clear the hash table + void clear(); +}; + +} // namespace detail +} // namespace coal + +#include "coal/broadphase/detail/sparse_hash_table-inl.h" + +#endif diff --git a/include/coal/broadphase/detail/spatial_hash-inl.h b/include/coal/broadphase/detail/spatial_hash-inl.h new file mode 100644 index 000000000..cb9d6bd9a --- /dev/null +++ b/include/coal/broadphase/detail/spatial_hash-inl.h @@ -0,0 +1,80 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_SPATIALHASH_INL_H +#define COAL_BROADPHASE_SPATIALHASH_INL_H + +#include "coal/broadphase/detail/spatial_hash.h" +#include + +namespace coal { +namespace detail { + +//============================================================================== +SpatialHash::SpatialHash(const AABB& scene_limit_, CoalScalar cell_size_) + : cell_size(cell_size_), scene_limit(scene_limit_) { + width[0] = std::ceil(scene_limit.width() / cell_size); + width[1] = std::ceil(scene_limit.height() / cell_size); + width[2] = std::ceil(scene_limit.depth() / cell_size); +} + +//============================================================================== +std::vector SpatialHash::operator()(const AABB& aabb) const { + int min_x = std::floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size); + int max_x = std::ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size); + int min_y = std::floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size); + int max_y = std::ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size); + int min_z = std::floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size); + int max_z = std::ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size); + + std::vector keys((max_x - min_x) * (max_y - min_y) * + (max_z - min_z)); + int id = 0; + for (int x = min_x; x < max_x; ++x) { + for (int y = min_y; y < max_y; ++y) { + for (int z = min_z; z < max_z; ++z) { + keys[id++] = x + y * width[0] + z * width[0] * width[1]; + } + } + } + return keys; +} + +} // namespace detail +} // namespace coal + +#endif diff --git a/include/coal/broadphase/detail/spatial_hash.h b/include/coal/broadphase/detail/spatial_hash.h new file mode 100644 index 000000000..6d8646e38 --- /dev/null +++ b/include/coal/broadphase/detail/spatial_hash.h @@ -0,0 +1,63 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_BROADPHASE_SPATIALHASH_H +#define COAL_BROADPHASE_SPATIALHASH_H + +#include "coal/BV/AABB.h" +#include + +namespace coal { + +namespace detail { + +/// @brief Spatial hash function: hash an AABB to a set of integer values +struct COAL_DLLAPI SpatialHash { + SpatialHash(const AABB& scene_limit_, CoalScalar cell_size_); + + std::vector operator()(const AABB& aabb) const; + + private: + CoalScalar cell_size; + AABB scene_limit; + unsigned int width[3]; +}; + +} // namespace detail +} // namespace coal + +#endif diff --git a/include/coal/collision.h b/include/coal/collision.h new file mode 100644 index 000000000..cf9b9a1c0 --- /dev/null +++ b/include/coal/collision.h @@ -0,0 +1,120 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2021, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_COLLISION_H +#define COAL_COLLISION_H + +#include "coal/data_types.h" +#include "coal/collision_object.h" +#include "coal/collision_data.h" +#include "coal/collision_func_matrix.h" +#include "coal/timings.h" + +namespace coal { + +/// @brief Main collision interface: given two collision objects, and the +/// requirements for contacts, including num of max contacts, whether perform +/// exhaustive collision (i.e., returning returning all the contact points), +/// whether return detailed contact information (i.e., normal, contact point, +/// depth; otherwise only contact primitive id is returned), this function +/// performs the collision between them. +/// Return value is the number of contacts generated between the two objects. +COAL_DLLAPI std::size_t collide(const CollisionObject* o1, + const CollisionObject* o2, + const CollisionRequest& request, + CollisionResult& result); + +/// @copydoc collide(const CollisionObject*, const CollisionObject*, const +/// CollisionRequest&, CollisionResult&) +COAL_DLLAPI std::size_t collide(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, + const CollisionRequest& request, + CollisionResult& result); + +/// @brief This class reduces the cost of identifying the geometry pair. +/// This is mostly useful for repeated shape-shape queries. +/// +/// \code +/// ComputeCollision calc_collision (o1, o2); +/// std::size_t ncontacts = calc_collision(tf1, tf2, request, result); +/// \endcode +class COAL_DLLAPI ComputeCollision { + public: + /// @brief Default constructor from two Collision Geometries. + ComputeCollision(const CollisionGeometry* o1, const CollisionGeometry* o2); + + std::size_t operator()(const Transform3s& tf1, const Transform3s& tf2, + const CollisionRequest& request, + CollisionResult& result) const; + + bool operator==(const ComputeCollision& other) const { + return o1 == other.o1 && o2 == other.o2 && solver == other.solver; + } + + bool operator!=(const ComputeCollision& other) const { + return !(*this == other); + } + + virtual ~ComputeCollision() {}; + + protected: + // These pointers are made mutable to let the derived classes to update + // their values when updating the collision geometry (e.g. creating a new + // one). This feature should be used carefully to avoid any mis usage (e.g, + // changing the type of the collision geometry should be avoided). + mutable const CollisionGeometry* o1; + mutable const CollisionGeometry* o2; + + mutable GJKSolver solver; + + CollisionFunctionMatrix::CollisionFunc func; + bool swap_geoms; + + virtual std::size_t run(const Transform3s& tf1, const Transform3s& tf2, + const CollisionRequest& request, + CollisionResult& result) const; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace coal + +#endif diff --git a/include/coal/collision_data.h b/include/coal/collision_data.h new file mode 100644 index 000000000..e885e66a6 --- /dev/null +++ b/include/coal/collision_data.h @@ -0,0 +1,1237 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2024, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_COLLISION_DATA_H +#define COAL_COLLISION_DATA_H + +#include +#include +#include +#include +#include + +#include "coal/collision_object.h" +#include "coal/config.hh" +#include "coal/data_types.h" +#include "coal/timings.h" +#include "coal/narrowphase/narrowphase_defaults.h" +#include "coal/logging.h" + +namespace coal { + +/// @brief Contact information returned by collision +struct COAL_DLLAPI Contact { + /// @brief collision object 1 + const CollisionGeometry* o1; + + /// @brief collision object 2 + const CollisionGeometry* o2; + + /// @brief contact primitive in object 1 + /// if object 1 is mesh or point cloud, it is the triangle or point id + /// if object 1 is geometry shape, it is NONE (-1), + /// if object 1 is octree, it is the id of the cell + int b1; + + /// @brief contact primitive in object 2 + /// if object 2 is mesh or point cloud, it is the triangle or point id + /// if object 2 is geometry shape, it is NONE (-1), + /// if object 2 is octree, it is the id of the cell + int b2; + + /// @brief contact normal, pointing from o1 to o2. + /// The normal defined as the normalized separation vector: + /// normal = (p2 - p1) / dist(o1, o2), where p1 = nearest_points[0] + /// belongs to o1 and p2 = nearest_points[1] belongs to o2 and dist(o1, o2) is + /// the **signed** distance between o1 and o2. The normal always points from + /// o1 to o2. + /// @note The separation vector is the smallest vector such that if o1 is + /// translated by it, o1 and o2 are in touching contact (they share at least + /// one contact point but have a zero intersection volume). If the shapes + /// overlap, dist(o1, o2) = -((p2-p1).norm()). Otherwise, dist(o1, o2) = + /// (p2-p1).norm(). + Vec3s normal; + + /// @brief nearest points associated to this contact. + /// @note Also referred as "witness points" in other collision libraries. + /// The points p1 = nearest_points[0] and p2 = nearest_points[1] verify the + /// property that dist(o1, o2) * (p1 - p2) is the separation vector between o1 + /// and o2, with dist(o1, o2) being the **signed** distance separating o1 from + /// o2. See \ref DistanceResult::normal for the definition of the separation + /// vector. If o1 and o2 have multiple contacts, the nearest_points are + /// associated with the contact which has the greatest penetration depth. + /// TODO (louis): rename `nearest_points` to `witness_points`. + std::array nearest_points; + + /// @brief contact position, in world space + Vec3s pos; + + /// @brief penetration depth + CoalScalar penetration_depth; + + /// @brief invalid contact primitive information + static const int NONE = -1; + + /// @brief Default constructor + Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) { + penetration_depth = (std::numeric_limits::max)(); + nearest_points[0] = nearest_points[1] = normal = pos = + Vec3s::Constant(std::numeric_limits::quiet_NaN()); + } + + Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, + int b2_) + : o1(o1_), o2(o2_), b1(b1_), b2(b2_) { + penetration_depth = (std::numeric_limits::max)(); + nearest_points[0] = nearest_points[1] = normal = pos = + Vec3s::Constant(std::numeric_limits::quiet_NaN()); + } + + Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, + int b2_, const Vec3s& pos_, const Vec3s& normal_, CoalScalar depth_) + : o1(o1_), + o2(o2_), + b1(b1_), + b2(b2_), + normal(normal_), + nearest_points{pos_ - (depth_ * normal_ / 2), + pos_ + (depth_ * normal_ / 2)}, + pos(pos_), + penetration_depth(depth_) {} + + Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, + int b2_, const Vec3s& p1, const Vec3s& p2, const Vec3s& normal_, + CoalScalar depth_) + : o1(o1_), + o2(o2_), + b1(b1_), + b2(b2_), + normal(normal_), + nearest_points{p1, p2}, + pos((p1 + p2) / 2), + penetration_depth(depth_) {} + + bool operator<(const Contact& other) const { + if (b1 == other.b1) return b2 < other.b2; + return b1 < other.b1; + } + + bool operator==(const Contact& other) const { + return o1 == other.o1 && o2 == other.o2 && b1 == other.b1 && + b2 == other.b2 && normal == other.normal && pos == other.pos && + nearest_points[0] == other.nearest_points[0] && + nearest_points[1] == other.nearest_points[1] && + penetration_depth == other.penetration_depth; + } + + bool operator!=(const Contact& other) const { return !(*this == other); } + + CoalScalar getDistanceToCollision(const CollisionRequest& request) const; +}; + +struct QueryResult; + +/// @brief base class for all query requests +struct COAL_DLLAPI QueryRequest { + // @brief Initial guess to use for the GJK algorithm + GJKInitialGuess gjk_initial_guess; + + /// @brief whether enable gjk initial guess + /// @Deprecated Use gjk_initial_guess instead + COAL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead) + bool enable_cached_gjk_guess; + + /// @brief the gjk initial guess set by user + mutable Vec3s cached_gjk_guess; + + /// @brief the support function initial guess set by user + mutable support_func_guess_t cached_support_func_guess; + + /// @brief maximum iteration for the GJK algorithm + size_t gjk_max_iterations; + + /// @brief tolerance for the GJK algorithm. + /// Note: This tolerance determines the precision on the estimated distance + /// between two geometries which are not in collision. + /// It is recommended to not set this tolerance to less than 1e-6. + CoalScalar gjk_tolerance; + + /// @brief whether to enable the Nesterov accleration of GJK + GJKVariant gjk_variant; + + /// @brief convergence criterion used to stop GJK + GJKConvergenceCriterion gjk_convergence_criterion; + + /// @brief convergence criterion used to stop GJK + GJKConvergenceCriterionType gjk_convergence_criterion_type; + + /// @brief max number of iterations for EPA + size_t epa_max_iterations; + + /// @brief tolerance for EPA. + /// Note: This tolerance determines the precision on the estimated distance + /// between two geometries which are in collision. + /// It is recommended to not set this tolerance to less than 1e-6. + /// Also, setting EPA's tolerance to less than GJK's is not recommended. + CoalScalar epa_tolerance; + + /// @brief enable timings when performing collision/distance request + bool enable_timings; + + /// @brief threshold below which a collision is considered. + CoalScalar collision_distance_threshold; + + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + /// @brief Default constructor. + QueryRequest() + : gjk_initial_guess(GJKInitialGuess::DefaultGuess), + enable_cached_gjk_guess(false), + cached_gjk_guess(1, 0, 0), + cached_support_func_guess(support_func_guess_t::Zero()), + gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS), + gjk_tolerance(GJK_DEFAULT_TOLERANCE), + gjk_variant(GJKVariant::DefaultGJK), + gjk_convergence_criterion(GJKConvergenceCriterion::Default), + gjk_convergence_criterion_type(GJKConvergenceCriterionType::Relative), + epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS), + epa_tolerance(EPA_DEFAULT_TOLERANCE), + enable_timings(false), + collision_distance_threshold( + Eigen::NumTraits::dummy_precision()) {} + + /// @brief Copy constructor. + QueryRequest(const QueryRequest& other) = default; + + /// @brief Copy assignment operator. + QueryRequest& operator=(const QueryRequest& other) = default; + COAL_COMPILER_DIAGNOSTIC_POP + + /// @brief Updates the guess for the internal GJK algorithm in order to + /// warm-start it when reusing this collision request on the same collision + /// pair. + /// @note The option `gjk_initial_guess` must be set to + /// `GJKInitialGuess::CachedGuess` for this to work. + void updateGuess(const QueryResult& result) const; + + /// @brief whether two QueryRequest are the same or not + inline bool operator==(const QueryRequest& other) const { + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + return gjk_initial_guess == other.gjk_initial_guess && + enable_cached_gjk_guess == other.enable_cached_gjk_guess && + gjk_variant == other.gjk_variant && + gjk_convergence_criterion == other.gjk_convergence_criterion && + gjk_convergence_criterion_type == + other.gjk_convergence_criterion_type && + gjk_tolerance == other.gjk_tolerance && + gjk_max_iterations == other.gjk_max_iterations && + cached_gjk_guess == other.cached_gjk_guess && + cached_support_func_guess == other.cached_support_func_guess && + epa_max_iterations == other.epa_max_iterations && + epa_tolerance == other.epa_tolerance && + enable_timings == other.enable_timings && + collision_distance_threshold == other.collision_distance_threshold; + COAL_COMPILER_DIAGNOSTIC_POP + } +}; + +/// @brief base class for all query results +struct COAL_DLLAPI QueryResult { + /// @brief stores the last GJK ray when relevant. + Vec3s cached_gjk_guess; + + /// @brief stores the last support function vertex index, when relevant. + support_func_guess_t cached_support_func_guess; + + /// @brief timings for the given request + CPUTimes timings; + + QueryResult() + : cached_gjk_guess(Vec3s::Zero()), + cached_support_func_guess(support_func_guess_t::Constant(-1)) {} +}; + +inline void QueryRequest::updateGuess(const QueryResult& result) const { + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + if (gjk_initial_guess == GJKInitialGuess::CachedGuess || + enable_cached_gjk_guess) { + cached_gjk_guess = result.cached_gjk_guess; + cached_support_func_guess = result.cached_support_func_guess; + } + COAL_COMPILER_DIAGNOSTIC_POP +} + +struct CollisionResult; + +/// @brief flag declaration for specifying required params in CollisionResult +enum CollisionRequestFlag { + CONTACT = 0x00001, + DISTANCE_LOWER_BOUND = 0x00002, + NO_REQUEST = 0x01000 +}; + +/// @brief request to the collision algorithm +struct COAL_DLLAPI CollisionRequest : QueryRequest { + /// @brief The maximum number of contacts that can be returned + size_t num_max_contacts; + + /// @brief whether the contact information (normal, penetration depth and + /// contact position) will return. + bool enable_contact; + + /// Whether a lower bound on distance is returned when objects are disjoint + COAL_DEPRECATED_MESSAGE(A lower bound on distance is always computed.) + bool enable_distance_lower_bound; + + /// @brief Distance below which objects are considered in collision. + /// See \ref coal_collision_and_distance_lower_bound_computation + /// @note If set to -inf, the objects tested for collision are considered + /// as collision free and no test is actually performed by functions + /// coal::collide of class coal::ComputeCollision. + CoalScalar security_margin; + + /// @brief Distance below which bounding volumes are broken down. + /// See \ref coal_collision_and_distance_lower_bound_computation + CoalScalar break_distance; + + /// @brief Distance above which GJK solver makes an early stopping. + /// GJK stops searching for the closest points when it proves that the + /// distance between two geometries is above this threshold. + /// + /// @remarks Consequently, the closest points might be incorrect, but allows + /// to save computational resources. + CoalScalar distance_upper_bound; + + /// @brief Constructor from a flag and a maximal number of contacts. + /// + /// @param[in] flag Collision request flag + /// @param[in] num_max_contacts Maximal number of allowed contacts + /// + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_) + : num_max_contacts(num_max_contacts_), + enable_contact(flag & CONTACT), + enable_distance_lower_bound(flag & DISTANCE_LOWER_BOUND), + security_margin(0), + break_distance(1e-3), + distance_upper_bound((std::numeric_limits::max)()) {} + + /// @brief Default constructor. + CollisionRequest() + : num_max_contacts(1), + enable_contact(true), + enable_distance_lower_bound(false), + security_margin(0), + break_distance(1e-3), + distance_upper_bound((std::numeric_limits::max)()) {} + COAL_COMPILER_DIAGNOSTIC_POP + + bool isSatisfied(const CollisionResult& result) const; + + /// @brief whether two CollisionRequest are the same or not + inline bool operator==(const CollisionRequest& other) const { + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + return QueryRequest::operator==(other) && + num_max_contacts == other.num_max_contacts && + enable_contact == other.enable_contact && + enable_distance_lower_bound == other.enable_distance_lower_bound && + security_margin == other.security_margin && + break_distance == other.break_distance && + distance_upper_bound == other.distance_upper_bound; + COAL_COMPILER_DIAGNOSTIC_POP + } +}; + +inline CoalScalar Contact::getDistanceToCollision( + const CollisionRequest& request) const { + return penetration_depth - request.security_margin; +} + +/// @brief collision result +struct COAL_DLLAPI CollisionResult : QueryResult { + private: + /// @brief contact information + std::vector contacts; + + public: + /// Lower bound on distance between objects if they are disjoint. + /// See \ref coal_collision_and_distance_lower_bound_computation + /// @note Always computed. If \ref CollisionRequest::distance_upper_bound is + /// set to infinity, distance_lower_bound is the actual distance between the + /// shapes. + CoalScalar distance_lower_bound; + + /// @brief normal associated to nearest_points. + /// Same as `CollisionResult::nearest_points` but for the normal. + Vec3s normal; + + /// @brief nearest points. + /// A `CollisionResult` can have multiple contacts. + /// The nearest points in `CollisionResults` correspond to the witness points + /// associated with the smallest distance i.e the `distance_lower_bound`. + /// For bounding volumes and BVHs, these nearest points are available + /// only when distance_lower_bound is inferior to + /// CollisionRequest::break_distance. + std::array nearest_points; + + public: + CollisionResult() + : distance_lower_bound((std::numeric_limits::max)()) { + nearest_points[0] = nearest_points[1] = normal = + Vec3s::Constant(std::numeric_limits::quiet_NaN()); + } + + /// @brief Update the lower bound only if the distance is inferior. + inline void updateDistanceLowerBound( + const CoalScalar& distance_lower_bound_) { + if (distance_lower_bound_ < distance_lower_bound) + distance_lower_bound = distance_lower_bound_; + } + + /// @brief add one contact into result structure + inline void addContact(const Contact& c) { contacts.push_back(c); } + + /// @brief whether two CollisionResult are the same or not + inline bool operator==(const CollisionResult& other) const { + return contacts == other.contacts && + distance_lower_bound == other.distance_lower_bound && + nearest_points[0] == other.nearest_points[0] && + nearest_points[1] == other.nearest_points[1] && + normal == other.normal; + } + + /// @brief return binary collision result + bool isCollision() const { return contacts.size() > 0; } + + /// @brief number of contacts found + size_t numContacts() const { return contacts.size(); } + + /// @brief get the i-th contact calculated + const Contact& getContact(size_t i) const { + if (contacts.size() == 0) + COAL_THROW_PRETTY( + "The number of contacts is zero. No Contact can be returned.", + std::invalid_argument); + + if (i < contacts.size()) + return contacts[i]; + else + return contacts.back(); + } + + /// @brief set the i-th contact calculated + void setContact(size_t i, const Contact& c) { + if (contacts.size() == 0) + COAL_THROW_PRETTY( + "The number of contacts is zero. No Contact can be returned.", + std::invalid_argument); + + if (i < contacts.size()) + contacts[i] = c; + else + contacts.back() = c; + } + + /// @brief get all the contacts + void getContacts(std::vector& contacts_) const { + contacts_.resize(contacts.size()); + std::copy(contacts.begin(), contacts.end(), contacts_.begin()); + } + + const std::vector& getContacts() const { return contacts; } + + /// @brief clear the results obtained + void clear() { + distance_lower_bound = (std::numeric_limits::max)(); + contacts.clear(); + timings.clear(); + nearest_points[0] = nearest_points[1] = normal = + Vec3s::Constant(std::numeric_limits::quiet_NaN()); + } + + /// @brief reposition Contact objects when fcl inverts them + /// during their construction. + void swapObjects(); +}; + +/// @brief This structure allows to encode contact patches. +/// A contact patch is defined by a set of points belonging to a subset of a +/// plane passing by `p` and supported by `n`, where `n = Contact::normal` and +/// `p = Contact::pos`. If we denote by P this plane and by S1 and S2 the first +/// and second shape of a collision pair, a contact patch is represented as a +/// polytope which vertices all belong to `P & S1 & S2`, where `&` denotes the +/// set-intersection. Since a contact patch is a subset of a plane supported by +/// `n`, it has a preferred direction. In Coal, the `Contact::normal` points +/// from S1 to S2. In the same way, a contact patch points by default from S1 +/// to S2. +/// +/// @note For now (April 2024), a `ContactPatch` is a polygon (2D polytope), +/// so the points of the set, forming the convex-hull of the polytope, are +/// stored in a counter-clockwise fashion. +/// @note If needed, the internal algorithms of Coal can easily be extended +/// to compute a contact volume instead of a contact patch. +struct COAL_DLLAPI ContactPatch { + public: + using Polygon = std::vector; + + /// @brief Frame of the set, expressed in the world coordinates. + /// The z-axis of the frame's rotation is the contact patch normal. + Transform3s tf; + + /// @brief Direction of ContactPatch. + /// When doing collision detection, the convention of Coal is that the + /// normal always points from the first to the second shape of the collision + /// pair i.e. from shape1 to shape2 when calling `collide(shape1, shape2)`. + /// The PatchDirection enum allows to identify if the patch points from + /// shape 1 to shape 2 (Default type) or from shape 2 to shape 1 (Inverted + /// type). The Inverted type should only be used for internal Coal + /// computations (it allows to properly define two separate contact patches in + /// the same frame). + enum PatchDirection { DEFAULT = 0, INVERTED = 1 }; + + /// @brief Direction of this contact patch. + PatchDirection direction; + + /// @brief Penetration depth of the contact patch. This value corresponds to + /// the signed distance `d` between the shapes. + /// @note For each contact point `p` in the patch of normal `n`, `p1 = p - + /// 0.5*d*n` and `p2 = p + 0.5*d*n` define a pair of witness points. `p1` + /// belongs to the surface of the first shape and `p2` belongs to the surface + /// of the second shape. For any pair of witness points, we always have `p2 - + /// p1 = d * n`. The vector `d * n` is called a minimum separation vector: + /// if S1 is translated by it, S1 and S2 are not in collision anymore. + /// @note Although there may exist multiple minimum separation vectors between + /// two shapes, the term "minimum" comes from the fact that it's impossible to + /// find a different separation vector which has a smaller norm than `d * n`. + CoalScalar penetration_depth; + + /// @brief Default maximum size of the polygon representing the contact patch. + /// Used to pre-allocate memory for the patch. + static constexpr size_t default_preallocated_size = 12; + + protected: + /// @brief Container for the vertices of the set. + Polygon m_points; + + public: + /// @brief Default constructor. + /// Note: the preallocated size does not determine the maximum number of + /// points in the patch, it only serves as preallocation if the maximum size + /// of the patch is known in advance. Coal will automatically expand/shrink + /// the contact patch if needed. + explicit ContactPatch(size_t preallocated_size = default_preallocated_size) + : tf(Transform3s::Identity()), + direction(PatchDirection::DEFAULT), + penetration_depth(0) { + this->m_points.reserve(preallocated_size); + } + + /// @brief Normal of the contact patch, expressed in the WORLD frame. + Vec3s getNormal() const { + if (this->direction == PatchDirection::INVERTED) { + return -this->tf.rotation().col(2); + } + return this->tf.rotation().col(2); + } + + /// @brief Returns the number of points in the contact patch. + size_t size() const { return this->m_points.size(); } + + /// @brief Add a 3D point to the set, expressed in the world frame. + /// @note This function takes a 3D point and expresses it in the local frame + /// of the set. It then takes only the x and y components of the vector, + /// effectively doing a projection onto the plane to which the set belongs. + /// TODO(louis): if necessary, we can store the offset to the plane (x, y). + void addPoint(const Vec3s& point_3d) { + const Vec3s point = this->tf.inverseTransform(point_3d); + this->m_points.emplace_back(point.template head<2>()); + } + + /// @brief Get the i-th point of the set, expressed in the 3D world frame. + Vec3s getPoint(const size_t i) const { + Vec3s point(0, 0, 0); + point.head<2>() = this->point(i); + point = tf.transform(point); + return point; + } + + /// @brief Get the i-th point of the contact patch, projected back onto the + /// first shape of the collision pair. This point is expressed in the 3D + /// world frame. + Vec3s getPointShape1(const size_t i) const { + Vec3s point = this->getPoint(i); + point -= (this->penetration_depth / 2) * this->getNormal(); + return point; + } + + /// @brief Get the i-th point of the contact patch, projected back onto the + /// first shape of the collision pair. This 3D point is expressed in the world + /// frame. + Vec3s getPointShape2(const size_t i) const { + Vec3s point = this->getPoint(i); + point += (this->penetration_depth / 2) * this->getNormal(); + return point; + } + + /// @brief Getter for the 2D points in the set. + Polygon& points() { return this->m_points; } + + /// @brief Const getter for the 2D points in the set. + const Polygon& points() const { return this->m_points; } + + /// @brief Getter for the i-th 2D point in the set. + Vec2s& point(const size_t i) { + COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error); + if (i < this->m_points.size()) { + return this->m_points[i]; + } + return this->m_points.back(); + } + + /// @brief Const getter for the i-th 2D point in the set. + const Vec2s& point(const size_t i) const { + COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error); + if (i < this->m_points.size()) { + return this->m_points[i]; + } + return this->m_points.back(); + } + + /// @brief Clear the set. + void clear() { + this->m_points.clear(); + this->tf.setIdentity(); + this->penetration_depth = 0; + } + + /// @brief Whether two contact patches are the same or not. + /// @note This compares the two sets terms by terms. + /// However, two contact patches can be identical, but have a different + /// order for their points. Use `isEqual` in this case. + bool operator==(const ContactPatch& other) const { + return this->tf == other.tf && this->direction == other.direction && + this->penetration_depth == other.penetration_depth && + this->points() == other.points(); + } + + /// @brief Whether two contact patches are the same or not. + /// Checks for different order of the points. + bool isSame(const ContactPatch& other, + const CoalScalar tol = + Eigen::NumTraits::dummy_precision()) const { + // The x and y axis of the set are arbitrary, but the z axis is + // always the normal. The position of the origin of the frame is also + // arbitrary. So we only check if the normals are the same. + if (!this->getNormal().isApprox(other.getNormal(), tol)) { + return false; + } + + if (std::abs(this->penetration_depth - other.penetration_depth) > tol) { + return false; + } + + if (this->direction != other.direction) { + return false; + } + + if (this->size() != other.size()) { + return false; + } + + // Check all points of the contact patch. + for (size_t i = 0; i < this->size(); ++i) { + bool found = false; + const Vec3s pi = this->getPoint(i); + for (size_t j = 0; j < other.size(); ++j) { + const Vec3s other_pj = other.getPoint(j); + if (pi.isApprox(other_pj, tol)) { + found = true; + } + } + if (!found) { + return false; + } + } + + return true; + } +}; + +/// @brief Construct a frame from a `Contact`'s position and normal. +/// Because both `Contact`'s position and normal are expressed in the world +/// frame, this frame is also expressed w.r.t the world frame. +/// The origin of the frame is `contact.pos` and the z-axis of the frame is +/// `contact.normal`. +inline void constructContactPatchFrameFromContact(const Contact& contact, + ContactPatch& contact_patch) { + contact_patch.penetration_depth = contact.penetration_depth; + contact_patch.tf.translation() = contact.pos; + contact_patch.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + contact_patch.direction = ContactPatch::PatchDirection::DEFAULT; +} + +/// @brief Structure used for internal computations. A support set and a +/// contact patch can be represented by the same structure. In fact, a contact +/// patch is the intersection of two support sets, one with +/// `PatchDirection::DEFAULT` and one with `PatchDirection::INVERTED`. +/// @note A support set with `DEFAULT` direction is the support set of a shape +/// in the direction given by `+n`, where `n` is the z-axis of the frame's +/// patch rotation. An `INVERTED` support set is the support set of a shape in +/// the direction `-n`. +using SupportSet = ContactPatch; + +/// @brief Request for a contact patch computation. +struct COAL_DLLAPI ContactPatchRequest { + /// @brief Maximum number of contact patches that will be computed. + size_t max_num_patch; + + protected: + /// @brief Maximum samples to compute the support sets of curved shapes, + /// i.e. when the normal is perpendicular to the base of a cylinder. For + /// now, only relevant for Cone and Cylinder. In the future this might be + /// extended to Sphere and Ellipsoid. + size_t m_num_samples_curved_shapes; + + /// @brief Tolerance below which points are added to a contact patch. + /// In details, given two shapes S1 and S2, a contact patch is the triple + /// intersection between the separating plane (P) (passing by `Contact::pos` + /// and supported by `Contact::normal`), S1 and S2; i.e. a contact patch is + /// `P & S1 & S2` if we denote `&` the set intersection operator. If a point + /// p1 of S1 is at a distance below `patch_tolerance` from the separating + /// plane, it is taken into account in the computation of the contact patch. + /// Otherwise, it is not used for the computation. + /// @note Needs to be positive. + CoalScalar m_patch_tolerance; + + public: + /// @brief Default constructor. + /// @param max_num_patch maximum number of contact patches per collision pair. + /// @param max_sub_patch_size maximum size of each sub contact patch. Each + /// contact patch contains an internal representation for an inscribed sub + /// contact patch. This allows physics simulation to always work with a + /// predetermined maximum size for each contact patch. A sub contact patch is + /// simply a subset of the vertices of a contact patch. + /// @param num_samples_curved_shapes for shapes like cones and cylinders, + /// which have smooth basis (circles in this case), we need to sample a + /// certain amount of point of this basis. + /// @param patch_tolerance the tolerance below which a point of a shape is + /// considered to belong to the support set of this shape in the direction of + /// the normal. Said otherwise, `patch_tolerance` determines the "thickness" + /// of the separating plane between shapes of a collision pair. + explicit ContactPatchRequest(size_t max_num_patch = 1, + size_t num_samples_curved_shapes = + ContactPatch::default_preallocated_size, + CoalScalar patch_tolerance = 1e-3) + : max_num_patch(max_num_patch) { + this->setNumSamplesCurvedShapes(num_samples_curved_shapes); + this->setPatchTolerance(patch_tolerance); + } + + /// @brief Construct a contact patch request from a collision request. + explicit ContactPatchRequest(const CollisionRequest& collision_request, + size_t num_samples_curved_shapes = + ContactPatch::default_preallocated_size, + CoalScalar patch_tolerance = 1e-3) + : max_num_patch(collision_request.num_max_contacts) { + this->setNumSamplesCurvedShapes(num_samples_curved_shapes); + this->setPatchTolerance(patch_tolerance); + } + + /// @copydoc m_num_samples_curved_shapes + void setNumSamplesCurvedShapes(const size_t num_samples_curved_shapes) { + if (num_samples_curved_shapes < 3) { + COAL_LOG_WARNING( + "`num_samples_curved_shapes` cannot be lower than 3. Setting it to " + "3 to prevent bugs."); + this->m_num_samples_curved_shapes = 3; + } else { + this->m_num_samples_curved_shapes = num_samples_curved_shapes; + } + } + + /// @copydoc m_num_samples_curved_shapes + size_t getNumSamplesCurvedShapes() const { + return this->m_num_samples_curved_shapes; + } + + /// @copydoc m_patch_tolerance + void setPatchTolerance(const CoalScalar patch_tolerance) { + if (patch_tolerance < 0) { + COAL_LOG_WARNING( + "`patch_tolerance` cannot be negative. Setting it to 0 to prevent " + "bugs."); + this->m_patch_tolerance = Eigen::NumTraits::dummy_precision(); + } else { + this->m_patch_tolerance = patch_tolerance; + } + } + + /// @copydoc m_patch_tolerance + CoalScalar getPatchTolerance() const { return this->m_patch_tolerance; } + + /// @brief Whether two ContactPatchRequest are identical or not. + bool operator==(const ContactPatchRequest& other) const { + return this->max_num_patch == other.max_num_patch && + this->getNumSamplesCurvedShapes() == + other.getNumSamplesCurvedShapes() && + this->getPatchTolerance() == other.getPatchTolerance(); + } +}; + +/// @brief Result for a contact patch computation. +struct COAL_DLLAPI ContactPatchResult { + using ContactPatchVector = std::vector; + using ContactPatchRef = std::reference_wrapper; + using ContactPatchRefVector = std::vector; + + protected: + /// @brief Data container for the vector of contact patches. + /// @note Contrary to `CollisionResult` or `DistanceResult`, which have a + /// very small memory footprint, contact patches can contain relatively + /// large polytopes. In order to reuse a `ContactPatchResult` while avoiding + /// successive mallocs, we have a data container and a vector which points + /// to the currently active patches in this data container. + ContactPatchVector m_contact_patches_data; + + /// @brief Contact patches in `m_contact_patches_data` can have two + /// statuses: used or unused. This index tracks the first unused patch in + /// the `m_contact_patches_data` vector. + size_t m_id_available_patch; + + /// @brief Vector of contact patches of the result. + ContactPatchRefVector m_contact_patches; + + public: + /// @brief Default constructor. + ContactPatchResult() : m_id_available_patch(0) { + const size_t max_num_patch = 1; + const ContactPatchRequest request(max_num_patch); + this->set(request); + } + + /// @brief Constructor using a `ContactPatchRequest`. + explicit ContactPatchResult(const ContactPatchRequest& request) + : m_id_available_patch(0) { + this->set(request); + }; + + /// @brief Number of contact patches in the result. + size_t numContactPatches() const { return this->m_contact_patches.size(); } + + /// @brief Returns a new unused contact patch from the internal data vector. + ContactPatchRef getUnusedContactPatch() { + if (this->m_id_available_patch >= this->m_contact_patches_data.size()) { + COAL_LOG_WARNING( + "Trying to get an unused contact patch but all contact patches are " + "used. Increasing size of contact patches vector, at the cost of a " + "copy. You should increase `max_num_patch` in the " + "`ContactPatchRequest`."); + this->m_contact_patches_data.emplace_back( + this->m_contact_patches_data.back()); + this->m_contact_patches_data.back().clear(); + } + ContactPatch& contact_patch = + this->m_contact_patches_data[this->m_id_available_patch]; + contact_patch.clear(); + this->m_contact_patches.emplace_back(contact_patch); + ++(this->m_id_available_patch); + return this->m_contact_patches.back(); + } + + /// @brief Const getter for the i-th contact patch of the result. + const ContactPatch& getContactPatch(const size_t i) const { + if (this->m_contact_patches.empty()) { + COAL_THROW_PRETTY( + "The number of contact patches is zero. No ContactPatch can be " + "returned.", + std::invalid_argument); + } + if (i < this->m_contact_patches.size()) { + return this->m_contact_patches[i]; + } + return this->m_contact_patches.back(); + } + + /// @brief Getter for the i-th contact patch of the result. + ContactPatch& contactPatch(const size_t i) { + if (this->m_contact_patches.empty()) { + COAL_THROW_PRETTY( + "The number of contact patches is zero. No ContactPatch can be " + "returned.", + std::invalid_argument); + } + if (i < this->m_contact_patches.size()) { + return this->m_contact_patches[i]; + } + return this->m_contact_patches.back(); + } + + /// @brief Clears the contact patch result. + void clear() { + this->m_contact_patches.clear(); + this->m_id_available_patch = 0; + for (ContactPatch& patch : this->m_contact_patches_data) { + patch.clear(); + } + } + + /// @brief Set up a `ContactPatchResult` from a `ContactPatchRequest` + void set(const ContactPatchRequest& request) { + if (this->m_contact_patches_data.size() < request.max_num_patch) { + this->m_contact_patches_data.resize(request.max_num_patch); + } + for (ContactPatch& patch : this->m_contact_patches_data) { + patch.points().reserve(request.getNumSamplesCurvedShapes()); + } + this->clear(); + } + + /// @brief Return true if this `ContactPatchResult` is aligned with the + /// `ContactPatchRequest` given as input. + bool check(const ContactPatchRequest& request) const { + assert(this->m_contact_patches_data.size() >= request.max_num_patch); + if (this->m_contact_patches_data.size() < request.max_num_patch) { + return false; + } + + for (const ContactPatch& patch : this->m_contact_patches_data) { + if (patch.points().capacity() < request.getNumSamplesCurvedShapes()) { + assert(patch.points().capacity() >= + request.getNumSamplesCurvedShapes()); + return false; + } + } + return true; + } + + /// @brief Whether two ContactPatchResult are identical or not. + bool operator==(const ContactPatchResult& other) const { + if (this->numContactPatches() != other.numContactPatches()) { + return false; + } + + for (size_t i = 0; i < this->numContactPatches(); ++i) { + const ContactPatch& patch = this->getContactPatch(i); + const ContactPatch& other_patch = other.getContactPatch(i); + if (!(patch == other_patch)) { + return false; + } + } + + return true; + } + + /// @brief Repositions the ContactPatch when they get inverted during their + /// construction. + void swapObjects() { + // Create new transform: it's the reflection of `tf` along the z-axis. + // This corresponds to doing a PI rotation along the y-axis, i.e. the x-axis + // becomes -x-axis, y-axis stays the same, z-axis becomes -z-axis. + for (size_t i = 0; i < this->numContactPatches(); ++i) { + ContactPatch& patch = this->contactPatch(i); + patch.tf.rotation().col(0) *= -1.0; + patch.tf.rotation().col(2) *= -1.0; + + for (size_t j = 0; j < patch.size(); ++j) { + patch.point(i)(0) *= -1.0; // only invert the x-axis + } + } + } +}; + +struct DistanceResult; + +/// @brief request to the distance computation +struct COAL_DLLAPI DistanceRequest : QueryRequest { + /// @brief whether to return the nearest points. + /// Nearest points are always computed and are the points of the shapes that + /// achieve a distance of `DistanceResult::min_distance`. + COAL_DEPRECATED_MESSAGE( + Nearest points are always computed : they are the points of the shapes + that achieve a distance of + `DistanceResult::min_distance` + .\n Use `enable_signed_distance` if you want to compute a signed + minimum distance(and thus its corresponding nearest points) + .) + bool enable_nearest_points; + + /// @brief whether to compute the penetration depth when objects are in + /// collision. + /// Turning this off can save computation time if only the distance + /// when objects are disjoint is needed. + /// @note The minimum distance between the shapes is stored in + /// `DistanceResult::min_distance`. + /// If `enable_signed_distance` is off, `DistanceResult::min_distance` + /// is always positive. + /// If `enable_signed_distance` is on, `DistanceResult::min_distance` + /// can be positive or negative. + /// The nearest points are the points of the shapes that achieve + /// a distance of `DistanceResult::min_distance`. + bool enable_signed_distance; + + /// @brief error threshold for approximate distance + CoalScalar rel_err; // relative error, between 0 and 1 + CoalScalar abs_err; // absolute error + + /// \param enable_nearest_points_ enables the nearest points computation. + /// \param enable_signed_distance_ allows to compute the penetration depth + /// \param rel_err_ + /// \param abs_err_ + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + DistanceRequest(bool enable_nearest_points_ = true, + bool enable_signed_distance_ = true, + CoalScalar rel_err_ = 0.0, CoalScalar abs_err_ = 0.0) + : enable_nearest_points(enable_nearest_points_), + enable_signed_distance(enable_signed_distance_), + rel_err(rel_err_), + abs_err(abs_err_) {} + COAL_COMPILER_DIAGNOSTIC_POP + + bool isSatisfied(const DistanceResult& result) const; + + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + DistanceRequest& operator=(const DistanceRequest& other) = default; + COAL_COMPILER_DIAGNOSTIC_POP + + /// @brief whether two DistanceRequest are the same or not + inline bool operator==(const DistanceRequest& other) const { + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + return QueryRequest::operator==(other) && + enable_nearest_points == other.enable_nearest_points && + enable_signed_distance == other.enable_signed_distance && + rel_err == other.rel_err && abs_err == other.abs_err; + COAL_COMPILER_DIAGNOSTIC_POP + } +}; + +/// @brief distance result +struct COAL_DLLAPI DistanceResult : QueryResult { + public: + /// @brief minimum distance between two objects. + /// If two objects are in collision and + /// DistanceRequest::enable_signed_distance is activated, min_distance <= 0. + /// @note The nearest points are the points of the shapes that achieve a + /// distance of `DistanceResult::min_distance`. + CoalScalar min_distance; + + /// @brief normal. + Vec3s normal; + + /// @brief nearest points. + /// See CollisionResult::nearest_points. + std::array nearest_points; + + /// @brief collision object 1 + const CollisionGeometry* o1; + + /// @brief collision object 2 + const CollisionGeometry* o2; + + /// @brief information about the nearest point in object 1 + /// if object 1 is mesh or point cloud, it is the triangle or point id + /// if object 1 is geometry shape, it is NONE (-1), + /// if object 1 is octree, it is the id of the cell + int b1; + + /// @brief information about the nearest point in object 2 + /// if object 2 is mesh or point cloud, it is the triangle or point id + /// if object 2 is geometry shape, it is NONE (-1), + /// if object 2 is octree, it is the id of the cell + int b2; + + /// @brief invalid contact primitive information + static const int NONE = -1; + + DistanceResult( + CoalScalar min_distance_ = (std::numeric_limits::max)()) + : min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) { + const Vec3s nan( + Vec3s::Constant(std::numeric_limits::quiet_NaN())); + nearest_points[0] = nearest_points[1] = normal = nan; + } + + /// @brief add distance information into the result + void update(CoalScalar distance, const CollisionGeometry* o1_, + const CollisionGeometry* o2_, int b1_, int b2_) { + if (min_distance > distance) { + min_distance = distance; + o1 = o1_; + o2 = o2_; + b1 = b1_; + b2 = b2_; + } + } + + /// @brief add distance information into the result + void update(CoalScalar distance, const CollisionGeometry* o1_, + const CollisionGeometry* o2_, int b1_, int b2_, const Vec3s& p1, + const Vec3s& p2, const Vec3s& normal_) { + if (min_distance > distance) { + min_distance = distance; + o1 = o1_; + o2 = o2_; + b1 = b1_; + b2 = b2_; + nearest_points[0] = p1; + nearest_points[1] = p2; + normal = normal_; + } + } + + /// @brief add distance information into the result + void update(const DistanceResult& other_result) { + if (min_distance > other_result.min_distance) { + min_distance = other_result.min_distance; + o1 = other_result.o1; + o2 = other_result.o2; + b1 = other_result.b1; + b2 = other_result.b2; + nearest_points[0] = other_result.nearest_points[0]; + nearest_points[1] = other_result.nearest_points[1]; + normal = other_result.normal; + } + } + + /// @brief clear the result + void clear() { + const Vec3s nan( + Vec3s::Constant(std::numeric_limits::quiet_NaN())); + min_distance = (std::numeric_limits::max)(); + o1 = NULL; + o2 = NULL; + b1 = NONE; + b2 = NONE; + nearest_points[0] = nearest_points[1] = normal = nan; + timings.clear(); + } + + /// @brief whether two DistanceResult are the same or not + inline bool operator==(const DistanceResult& other) const { + bool is_same = min_distance == other.min_distance && + nearest_points[0] == other.nearest_points[0] && + nearest_points[1] == other.nearest_points[1] && + normal == other.normal && o1 == other.o1 && o2 == other.o2 && + b1 == other.b1 && b2 == other.b2; + + // TODO: check also that two GeometryObject are indeed equal. + if ((o1 != NULL) ^ (other.o1 != NULL)) return false; + is_same &= (o1 == other.o1); + // else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 == + // *other.o1; + + if ((o2 != NULL) ^ (other.o2 != NULL)) return false; + is_same &= (o2 == other.o2); + // else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 == + // *other.o2; + + return is_same; + } +}; + +namespace internal { +inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/, + CollisionResult& res, + const CoalScalar sqrDistLowerBound) { + // BV cannot find negative distance. + if (res.distance_lower_bound <= 0) return; + CoalScalar new_dlb = std::sqrt(sqrDistLowerBound); // - req.security_margin; + if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb; +} + +inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&, + CollisionResult& res, + const CoalScalar& distance, + const Vec3s& p0, const Vec3s& p1, + const Vec3s& normal) { + if (distance < res.distance_lower_bound) { + res.distance_lower_bound = distance; + res.nearest_points[0] = p0; + res.nearest_points[1] = p1; + res.normal = normal; + } +} +} // namespace internal + +inline CollisionRequestFlag operator~(CollisionRequestFlag a) { + return static_cast(~static_cast(a)); +} + +inline CollisionRequestFlag operator|(CollisionRequestFlag a, + CollisionRequestFlag b) { + return static_cast(static_cast(a) | + static_cast(b)); +} + +inline CollisionRequestFlag operator&(CollisionRequestFlag a, + CollisionRequestFlag b) { + return static_cast(static_cast(a) & + static_cast(b)); +} + +inline CollisionRequestFlag operator^(CollisionRequestFlag a, + CollisionRequestFlag b) { + return static_cast(static_cast(a) ^ + static_cast(b)); +} + +inline CollisionRequestFlag& operator|=(CollisionRequestFlag& a, + CollisionRequestFlag b) { + return (CollisionRequestFlag&)((int&)(a) |= static_cast(b)); +} + +inline CollisionRequestFlag& operator&=(CollisionRequestFlag& a, + CollisionRequestFlag b) { + return (CollisionRequestFlag&)((int&)(a) &= static_cast(b)); +} + +inline CollisionRequestFlag& operator^=(CollisionRequestFlag& a, + CollisionRequestFlag b) { + return (CollisionRequestFlag&)((int&)(a) ^= static_cast(b)); +} + +} // namespace coal + +#endif diff --git a/include/coal/collision_func_matrix.h b/include/coal/collision_func_matrix.h new file mode 100644 index 000000000..e9aeca48f --- /dev/null +++ b/include/coal/collision_func_matrix.h @@ -0,0 +1,77 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_COLLISION_FUNC_MATRIX_H +#define COAL_COLLISION_FUNC_MATRIX_H + +#include "coal/collision_object.h" +#include "coal/collision_data.h" +#include "coal/narrowphase/narrowphase.h" + +namespace coal { + +/// @brief collision matrix stores the functions for collision between different +/// types of objects and provides a uniform call interface + +struct COAL_DLLAPI CollisionFunctionMatrix { + /// @brief the uniform call interface for collision: for collision, we need + /// know + /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 + /// and tf2; + /// 2. the solver for narrow phase collision, this is for the collision + /// between geometric shapes; + /// 3. the request setting for collision (e.g., whether need to return normal + /// information, whether need to compute cost); + /// 4. the structure to return collision result + typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result); + + /// @brief each item in the collision matrix is a function to handle collision + /// between objects of type1 and type2 + CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT]; + + CollisionFunctionMatrix(); +}; + +} // namespace coal + +#endif diff --git a/include/coal/collision_object.h b/include/coal/collision_object.h new file mode 100644 index 000000000..ccc5cf136 --- /dev/null +++ b/include/coal/collision_object.h @@ -0,0 +1,360 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_COLLISION_OBJECT_BASE_H +#define COAL_COLLISION_OBJECT_BASE_H + +#include +#include + +#include "coal/deprecated.hh" +#include "coal/fwd.hh" +#include "coal/BV/AABB.h" +#include "coal/math/transform.h" + +namespace coal { + +/// @brief object type: BVH (mesh, points), basic geometry, octree +enum OBJECT_TYPE { + OT_UNKNOWN, + OT_BVH, + OT_GEOM, + OT_OCTREE, + OT_HFIELD, + OT_COUNT +}; + +/// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, +/// KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, +/// cylinder, convex, plane, triangle), and octree +enum NODE_TYPE { + BV_UNKNOWN, + BV_AABB, + BV_OBB, + BV_RSS, + BV_kIOS, + BV_OBBRSS, + BV_KDOP16, + BV_KDOP18, + BV_KDOP24, + GEOM_BOX, + GEOM_SPHERE, + GEOM_CAPSULE, + GEOM_CONE, + GEOM_CYLINDER, + GEOM_CONVEX, + GEOM_PLANE, + GEOM_HALFSPACE, + GEOM_TRIANGLE, + GEOM_OCTREE, + GEOM_ELLIPSOID, + HF_AABB, + HF_OBBRSS, + NODE_COUNT +}; + +/// @addtogroup Construction_Of_BVH +/// @{ + +/// @brief The geometry for the object for collision or distance computation +class COAL_DLLAPI CollisionGeometry { + public: + CollisionGeometry() + : aabb_center(Vec3s::Constant((std::numeric_limits::max)())), + aabb_radius(-1), + cost_density(1), + threshold_occupied(1), + threshold_free(0) {} + + /// \brief Copy constructor + CollisionGeometry(const CollisionGeometry& other) = default; + + virtual ~CollisionGeometry() {} + + /// @brief Clone *this into a new CollisionGeometry + virtual CollisionGeometry* clone() const = 0; + + /// \brief Equality operator + bool operator==(const CollisionGeometry& other) const { + return cost_density == other.cost_density && + threshold_occupied == other.threshold_occupied && + threshold_free == other.threshold_free && + aabb_center == other.aabb_center && + aabb_radius == other.aabb_radius && aabb_local == other.aabb_local && + isEqual(other); + } + + /// \brief Difference operator + bool operator!=(const CollisionGeometry& other) const { + return isNotEqual(other); + } + + /// @brief get the type of the object + virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; } + + /// @brief get the node type + virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; } + + /// @brief compute the AABB for object in local coordinate + virtual void computeLocalAABB() = 0; + + /// @brief get user data in geometry + void* getUserData() const { return user_data; } + + /// @brief set user data in geometry + void setUserData(void* data) { user_data = data; } + + /// @brief whether the object is completely occupied + inline bool isOccupied() const { return cost_density >= threshold_occupied; } + + /// @brief whether the object is completely free + inline bool isFree() const { return cost_density <= threshold_free; } + + /// @brief whether the object has some uncertainty + bool isUncertain() const; + + /// @brief AABB center in local coordinate + Vec3s aabb_center; + + /// @brief AABB radius + CoalScalar aabb_radius; + + /// @brief AABB in local coordinate, used for tight AABB when only translation + /// transform + AABB aabb_local; + + /// @brief pointer to user defined data specific to this object + void* user_data; + + /// @brief collision cost for unit volume + CoalScalar cost_density; + + /// @brief threshold for occupied ( >= is occupied) + CoalScalar threshold_occupied; + + /// @brief threshold for free (<= is free) + CoalScalar threshold_free; + + /// @brief compute center of mass + virtual Vec3s computeCOM() const { return Vec3s::Zero(); } + + /// @brief compute the inertia matrix, related to the origin + virtual Matrix3s computeMomentofInertia() const { + return Matrix3s::Constant(NAN); + } + + /// @brief compute the volume + virtual CoalScalar computeVolume() const { return 0; } + + /// @brief compute the inertia matrix, related to the com + virtual Matrix3s computeMomentofInertiaRelatedToCOM() const { + Matrix3s C = computeMomentofInertia(); + Vec3s com = computeCOM(); + CoalScalar V = computeVolume(); + + return (Matrix3s() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]), + C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2], + C(1, 0) + V * com[1] * com[0], + C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]), + C(1, 2) + V * com[1] * com[2], C(2, 0) + V * com[2] * com[0], + C(2, 1) + V * com[2] * com[1], + C(2, 2) - V * (com[0] * com[0] + com[1] * com[1])) + .finished(); + } + + private: + /// @brief equal operator with another object of derived type. + virtual bool isEqual(const CollisionGeometry& other) const = 0; + + /// @brief not equal operator with another object of derived type. + virtual bool isNotEqual(const CollisionGeometry& other) const { + return !(*this == other); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// @brief the object for collision or distance computation, contains the +/// geometry and the transform information +class COAL_DLLAPI CollisionObject { + public: + CollisionObject(const shared_ptr& cgeom_, + bool compute_local_aabb = true) + : cgeom(cgeom_), user_data(nullptr) { + init(compute_local_aabb); + } + + CollisionObject(const shared_ptr& cgeom_, + const Transform3s& tf, bool compute_local_aabb = true) + : cgeom(cgeom_), t(tf), user_data(nullptr) { + init(compute_local_aabb); + } + + CollisionObject(const shared_ptr& cgeom_, + const Matrix3s& R, const Vec3s& T, + bool compute_local_aabb = true) + : cgeom(cgeom_), t(R, T), user_data(nullptr) { + init(compute_local_aabb); + } + + bool operator==(const CollisionObject& other) const { + return cgeom == other.cgeom && t == other.t && user_data == other.user_data; + } + + bool operator!=(const CollisionObject& other) const { + return !(*this == other); + } + + ~CollisionObject() {} + + /// @brief get the type of the object + OBJECT_TYPE getObjectType() const { return cgeom->getObjectType(); } + + /// @brief get the node type + NODE_TYPE getNodeType() const { return cgeom->getNodeType(); } + + /// @brief get the AABB in world space + const AABB& getAABB() const { return aabb; } + + /// @brief get the AABB in world space + AABB& getAABB() { return aabb; } + + /// @brief compute the AABB in world space + void computeAABB() { + if (t.getRotation().isIdentity()) { + aabb = translate(cgeom->aabb_local, t.getTranslation()); + } else { + aabb.min_ = aabb.max_ = t.getTranslation(); + + Vec3s min_world, max_world; + for (int k = 0; k < 3; ++k) { + min_world.array() = t.getRotation().row(k).array() * + cgeom->aabb_local.min_.transpose().array(); + max_world.array() = t.getRotation().row(k).array() * + cgeom->aabb_local.max_.transpose().array(); + + aabb.min_[k] += (min_world.array().min)(max_world.array()).sum(); + aabb.max_[k] += (min_world.array().max)(max_world.array()).sum(); + } + } + } + + /// @brief get user data in object + void* getUserData() const { return user_data; } + + /// @brief set user data in object + void setUserData(void* data) { user_data = data; } + + /// @brief get translation of the object + inline const Vec3s& getTranslation() const { return t.getTranslation(); } + + /// @brief get matrix rotation of the object + inline const Matrix3s& getRotation() const { return t.getRotation(); } + + /// @brief get object's transform + inline const Transform3s& getTransform() const { return t; } + + /// @brief set object's rotation matrix + void setRotation(const Matrix3s& R) { t.setRotation(R); } + + /// @brief set object's translation + void setTranslation(const Vec3s& T) { t.setTranslation(T); } + + /// @brief set object's transform + void setTransform(const Matrix3s& R, const Vec3s& T) { t.setTransform(R, T); } + + /// @brief set object's transform + void setTransform(const Transform3s& tf) { t = tf; } + + /// @brief whether the object is in local coordinate + bool isIdentityTransform() const { return t.isIdentity(); } + + /// @brief set the object in local coordinate + void setIdentityTransform() { t.setIdentity(); } + + /// @brief get shared pointer to collision geometry of the object instance + const shared_ptr collisionGeometry() const { + return cgeom; + } + + /// @brief get shared pointer to collision geometry of the object instance + const shared_ptr& collisionGeometry() { return cgeom; } + + /// @brief get raw pointer to collision geometry of the object instance + const CollisionGeometry* collisionGeometryPtr() const { return cgeom.get(); } + + /// @brief get raw pointer to collision geometry of the object instance + CollisionGeometry* collisionGeometryPtr() { return cgeom.get(); } + + /// @brief Associate a new CollisionGeometry + /// + /// @param[in] collision_geometry The new CollisionGeometry + /// @param[in] compute_local_aabb Whether the local aabb of the input new has + /// to be computed. + /// + void setCollisionGeometry( + const shared_ptr& collision_geometry, + bool compute_local_aabb = true) { + if (collision_geometry.get() != cgeom.get()) { + cgeom = collision_geometry; + init(compute_local_aabb); + } + } + + protected: + void init(bool compute_local_aabb = true) { + if (cgeom) { + if (compute_local_aabb) cgeom->computeLocalAABB(); + computeAABB(); + } + } + + shared_ptr cgeom; + + Transform3s t; + + /// @brief AABB in global coordinate + mutable AABB aabb; + + /// @brief pointer to user defined data specific to this object + void* user_data; +}; + +} // namespace coal + +#endif diff --git a/include/coal/collision_utility.h b/include/coal/collision_utility.h new file mode 100644 index 000000000..44a597d26 --- /dev/null +++ b/include/coal/collision_utility.h @@ -0,0 +1,56 @@ +// Copyright (c) 2017 CNRS +// Copyright (c) 2022 INRIA +// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) +// +// This file is part of Coal. +// Coal is free software: you can redistribute it +// and/or modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation, either version +// 3 of the License, or (at your option) any later version. +// +// Coal is distributed in the hope that it will be +// useful, but WITHOUT ANY WARRANTY; without even the implied warranty +// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Lesser Public License for more details. You should have +// received a copy of the GNU Lesser General Public License along with +// Coal. If not, see . + +#ifndef COAL_COLLISION_UTILITY_H +#define COAL_COLLISION_UTILITY_H + +#include "coal/collision_object.h" + +namespace coal { + +COAL_DLLAPI CollisionGeometry* extract(const CollisionGeometry* model, + const Transform3s& pose, + const AABB& aabb); + +/** + * \brief Returns the name associated to a NODE_TYPE + */ +inline const char* get_node_type_name(NODE_TYPE node_type) { + static const char* node_type_name_all[] = { + "BV_UNKNOWN", "BV_AABB", "BV_OBB", "BV_RSS", + "BV_kIOS", "BV_OBBRSS", "BV_KDOP16", "BV_KDOP18", + "BV_KDOP24", "GEOM_BOX", "GEOM_SPHERE", "GEOM_CAPSULE", + "GEOM_CONE", "GEOM_CYLINDER", "GEOM_CONVEX", "GEOM_PLANE", + "GEOM_HALFSPACE", "GEOM_TRIANGLE", "GEOM_OCTREE", "GEOM_ELLIPSOID", + "HF_AABB", "HF_OBBRSS", "NODE_COUNT"}; + + return node_type_name_all[node_type]; +} + +/** + * \brief Returns the name associated to a OBJECT_TYPE + */ +inline const char* get_object_type_name(OBJECT_TYPE object_type) { + static const char* object_type_name_all[] = { + "OT_UNKNOWN", "OT_BVH", "OT_GEOM", "OT_OCTREE", "OT_HFIELD", "OT_COUNT"}; + + return object_type_name_all[object_type]; +} + +} // namespace coal + +#endif // COAL_COLLISION_UTILITY_H diff --git a/include/coal/contact_patch.h b/include/coal/contact_patch.h new file mode 100644 index 000000000..a942fa0b9 --- /dev/null +++ b/include/coal/contact_patch.h @@ -0,0 +1,122 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of INRIA nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Louis Montaut */ + +#ifndef COAL_CONTACT_PATCH_H +#define COAL_CONTACT_PATCH_H + +#include "coal/data_types.h" +#include "coal/collision_data.h" +#include "coal/contact_patch/contact_patch_solver.h" +#include "coal/contact_patch_func_matrix.h" + +namespace coal { + +/// @brief Main contact patch computation interface. +/// @note Please see @ref ContactPatchRequest and @ref ContactPatchResult for +/// more info on the content of the input/output of this function. Also, please +/// read @ref ContactPatch if you want to fully understand what is meant by +/// "contact patch". +COAL_DLLAPI void computeContactPatch(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, + const CollisionResult& collision_result, + const ContactPatchRequest& request, + ContactPatchResult& result); + +/// @copydoc computeContactPatch(const CollisionGeometry*, const Transform3s&, +// const CollisionGeometry*, const Transform3s&, const CollisionResult&, const +// ContactPatchRequest&, ContactPatchResult&); +COAL_DLLAPI void computeContactPatch(const CollisionObject* o1, + const CollisionObject* o2, + const CollisionResult& collision_result, + const ContactPatchRequest& request, + ContactPatchResult& result); + +/// @brief This class reduces the cost of identifying the geometry pair. +/// This is usefull for repeated shape-shape queries. +/// @note This needs to be called after `collide` or after `ComputeCollision`. +/// +/// \code +/// ComputeContactPatch calc_patch (o1, o2); +/// calc_patch(tf1, tf2, collision_result, patch_request, patch_result); +/// \endcode +class COAL_DLLAPI ComputeContactPatch { + public: + /// @brief Default constructor from two Collision Geometries. + ComputeContactPatch(const CollisionGeometry* o1, const CollisionGeometry* o2); + + void operator()(const Transform3s& tf1, const Transform3s& tf2, + const CollisionResult& collision_result, + const ContactPatchRequest& request, + ContactPatchResult& result) const; + + bool operator==(const ComputeContactPatch& other) const { + return this->o1 == other.o1 && this->o2 == other.o2 && + this->csolver == other.csolver; + } + + bool operator!=(const ComputeContactPatch& other) const { + return !(*this == other); + } + + virtual ~ComputeContactPatch() = default; + + protected: + // These pointers are made mutable to let the derived classes to update + // their values when updating the collision geometry (e.g. creating a new + // one). This feature should be used carefully to avoid any mis usage (e.g, + // changing the type of the collision geometry should be avoided). + mutable const CollisionGeometry* o1; + mutable const CollisionGeometry* o2; + + mutable ContactPatchSolver csolver; + + ContactPatchFunctionMatrix::ContactPatchFunc func; + bool swap_geoms; + + virtual void run(const Transform3s& tf1, const Transform3s& tf2, + const CollisionResult& collision_result, + const ContactPatchRequest& request, + ContactPatchResult& result) const; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace coal + +#endif diff --git a/include/coal/contact_patch/contact_patch_solver.h b/include/coal/contact_patch/contact_patch_solver.h new file mode 100644 index 000000000..cce436ea1 --- /dev/null +++ b/include/coal/contact_patch/contact_patch_solver.h @@ -0,0 +1,209 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, INRIA + * All rights reserved. + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of INRIA nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Louis Montaut */ + +#ifndef COAL_CONTACT_PATCH_SOLVER_H +#define COAL_CONTACT_PATCH_SOLVER_H + +#include "coal/collision_data.h" +#include "coal/logging.h" +#include "coal/narrowphase/gjk.h" + +namespace coal { + +/// @brief Solver to compute contact patches, i.e. the intersection between two +/// contact surfaces projected onto the shapes' separating plane. +/// Otherwise said, a contact patch is simply the intersection between two +/// support sets: the support set of shape S1 in direction `n` and the support +/// set of shape S2 in direction `-n`, where `n` is the contact normal +/// (satisfying the optimality conditions of GJK/EPA). +/// @note A contact patch is **not** the support set of the Minkowski Difference +/// in the direction of the normal. +/// A contact patch is actually the support set of the Minkowski difference in +/// the direction of the normal, i.e. the instersection of the shapes support +/// sets as mentioned above. +/// +/// TODO(louis): algo improvement: +/// - The clipping algo is currently n1 * n2; it can be done in n1 + n2. +struct COAL_DLLAPI ContactPatchSolver { + // Note: `ContactPatch` is an alias for `SupportSet`. + // The two can be used interchangeably. + using ShapeSupportData = details::ShapeSupportData; + using SupportSetDirection = SupportSet::PatchDirection; + + /// @brief Support set function for shape si. + /// @param[in] shape the shape. + /// @param[in/out] support_set a support set of the shape. A support set is + /// attached to a frame. All the points of the set computed by this function + /// will be expressed in the local frame of the support set. The support set + /// is computed in the direction of the positive z-axis if its direction is + /// DEFAULT, negative z-axis if its direction is INVERTED. + /// @param[in/out] hint for the support computation of ConvexBase shapes. Gets + /// updated after calling the function onto ConvexBase shapes. + /// @param[in/out] support_data for the support computation of ConvexBase + /// shapes. Gets updated with visited vertices after calling the function onto + /// ConvexBase shapes. + /// @param[in] num_sampled_supports for shapes like cone or cylinders which + /// have smooth non-strictly convex sides (their bases are circles), we need + /// to know how many supports we sample from these sides. For any other shape, + /// this parameter is not used. + /// @param[in] tol the "thickness" of the support plane. Any point v which + /// satisfies `max_{x in shape}(x.dot(dir)) - v.dot(dir) <= tol` is tol + /// distant from the support plane and is added to the support set. + typedef void (*SupportSetFunction)(const ShapeBase* shape, + SupportSet& support_set, int& hint, + ShapeSupportData& support_data, + size_t num_sampled_supports, + CoalScalar tol); + + /// @brief Number of vectors to pre-allocate in the `m_clipping_sets` vectors. + static constexpr size_t default_num_preallocated_supports = 16; + + /// @brief Number of points sampled for Cone and Cylinder when the normal is + /// orthogonal to the shapes' basis. + /// See @ref ContactPatchRequest::m_num_samples_curved_shapes for more + /// details. + size_t num_samples_curved_shapes; + + /// @brief Tolerance below which points are added to the shapes support sets. + /// See @ref ContactPatchRequest::m_patch_tolerance for more details. + CoalScalar patch_tolerance; + + /// @brief Support set function for shape s1. + mutable SupportSetFunction supportFuncShape1; + + /// @brief Support set function for shape s2. + mutable SupportSetFunction supportFuncShape2; + + /// @brief Temporary data to compute the support sets on each shape. + mutable std::array supports_data; + + /// @brief Guess for the support sets computation. + mutable support_func_guess_t support_guess; + + /// @brief Holder for support set of shape 1, used for internal computation. + /// After `computePatch` has been called, this support set is no longer valid. + mutable SupportSet support_set_shape1; + + /// @brief Holder for support set of shape 2, used for internal computation. + /// After `computePatch` has been called, this support set is no longer valid. + mutable SupportSet support_set_shape2; + + /// @brief Temporary support set used for the Sutherland-Hodgman algorithm. + mutable SupportSet support_set_buffer; + + /// @brief Tracks which point of the Sutherland-Hodgman result have been added + /// to the contact patch. Only used if the post-processing step occurs, i.e. + /// if the result of Sutherland-Hodgman has a size bigger than + /// `max_patch_size`. + mutable std::vector added_to_patch; + + /// @brief Default constructor. + explicit ContactPatchSolver() { + const size_t num_contact_patch = 1; + const size_t preallocated_patch_size = + ContactPatch::default_preallocated_size; + const CoalScalar patch_tolerance = 1e-3; + const ContactPatchRequest request(num_contact_patch, + preallocated_patch_size, patch_tolerance); + this->set(request); + } + + /// @brief Construct the solver with a `ContactPatchRequest`. + explicit ContactPatchSolver(const ContactPatchRequest& request) { + this->set(request); + } + + /// @brief Set up the solver using a `ContactPatchRequest`. + void set(const ContactPatchRequest& request); + + /// @brief Sets the support guess used during support set computation of + /// shapes s1 and s2. + void setSupportGuess(const support_func_guess_t guess) const { + this->support_guess = guess; + } + + /// @brief Main API of the solver: compute a contact patch from a contact + /// between shapes s1 and s2. + /// The contact patch is the (triple) intersection between the separating + /// plane passing (by `contact.pos` and supported by `contact.normal`) and the + /// shapes s1 and s2. + template + void computePatch(const ShapeType1& s1, const Transform3s& tf1, + const ShapeType2& s2, const Transform3s& tf2, + const Contact& contact, ContactPatch& contact_patch) const; + + /// @brief Reset the internal quantities of the solver. + template + void reset(const ShapeType1& shape1, const Transform3s& tf1, + const ShapeType2& shape2, const Transform3s& tf2, + const ContactPatch& contact_patch) const; + + /// @brief Retrieve result, adds a post-processing step if result has bigger + /// size than `this->max_patch_size`. + void getResult(const Contact& contact, const ContactPatch::Polygon* result, + ContactPatch& contact_patch) const; + + /// @return the intersecting point between line defined by ray (a, b) and + /// the segment [c, d]. + /// @note we make the following hypothesis: + /// 1) c != d (should be when creating initial polytopes) + /// 2) (c, d) is not parallel to ray -> if so, we return d. + static Vec2s computeLineSegmentIntersection(const Vec2s& a, const Vec2s& b, + const Vec2s& c, const Vec2s& d); + + /// @brief Construct support set function for shape. + static SupportSetFunction makeSupportSetFunction( + const ShapeBase* shape, ShapeSupportData& support_data); + + bool operator==(const ContactPatchSolver& other) const { + return this->num_samples_curved_shapes == other.num_samples_curved_shapes && + this->patch_tolerance == other.patch_tolerance && + this->support_guess == other.support_guess && + this->support_set_shape1 == other.support_set_shape1 && + this->support_set_shape2 == other.support_set_shape2 && + this->support_set_buffer == other.support_set_buffer && + this->added_to_patch == other.added_to_patch && + this->supportFuncShape1 == other.supportFuncShape1 && + this->supportFuncShape2 == other.supportFuncShape2; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace coal + +#include "coal/contact_patch/contact_patch_solver.hxx" + +#endif // COAL_CONTACT_PATCH_SOLVER_H diff --git a/include/coal/contact_patch/contact_patch_solver.hxx b/include/coal/contact_patch/contact_patch_solver.hxx new file mode 100644 index 000000000..f4f97b02c --- /dev/null +++ b/include/coal/contact_patch/contact_patch_solver.hxx @@ -0,0 +1,425 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of INRIA nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Louis Montaut */ + +#ifndef COAL_CONTACT_PATCH_SOLVER_HXX +#define COAL_CONTACT_PATCH_SOLVER_HXX + +#include "coal/data_types.h" +#include "coal/shape/geometric_shapes_traits.h" + +namespace coal { + +// ============================================================================ +inline void ContactPatchSolver::set(const ContactPatchRequest& request) { + // Note: it's important for the number of pre-allocated Vec3s in + // `m_clipping_sets` to be larger than `request.max_size_patch` + // because we don't know in advance how many supports will be discarded to + // form the convex-hulls of the shapes supports which will serve as the + // input of the Sutherland-Hodgman algorithm. + size_t num_preallocated_supports = default_num_preallocated_supports; + if (num_preallocated_supports < 2 * request.getNumSamplesCurvedShapes()) { + num_preallocated_supports = 2 * request.getNumSamplesCurvedShapes(); + } + + // Used for support set computation of shape1 and for the first iterate of the + // Sutherland-Hodgman algo. + this->support_set_shape1.points().reserve(num_preallocated_supports); + this->support_set_shape1.direction = SupportSetDirection::DEFAULT; + + // Used for computing the next iterate of the Sutherland-Hodgman algo. + this->support_set_buffer.points().reserve(num_preallocated_supports); + + // Used for support set computation of shape2 and acts as the "clipper" set in + // the Sutherland-Hodgman algo. + this->support_set_shape2.points().reserve(num_preallocated_supports); + this->support_set_shape2.direction = SupportSetDirection::INVERTED; + + this->num_samples_curved_shapes = request.getNumSamplesCurvedShapes(); + this->patch_tolerance = request.getPatchTolerance(); +} + +// ============================================================================ +template +void ContactPatchSolver::computePatch(const ShapeType1& s1, + const Transform3s& tf1, + const ShapeType2& s2, + const Transform3s& tf2, + const Contact& contact, + ContactPatch& contact_patch) const { + // Note: `ContactPatch` is an alias for `SupportSet`. + // Step 1 + constructContactPatchFrameFromContact(contact, contact_patch); + contact_patch.points().clear(); + if ((bool)(shape_traits::IsStrictlyConvex) || + (bool)(shape_traits::IsStrictlyConvex)) { + // If a shape is strictly convex, the support set in any direction is + // reduced to a single point. Thus, the contact point `contact.pos` is the + // only point belonging to the contact patch, and it has already been + // computed. + // TODO(louis): even for strictly convex shapes, we can sample the support + // function around the normal and return a pseudo support set. This would + // allow spheres and ellipsoids to have a contact surface, which does make + // sense in certain physics simulation cases. + // Do the same for strictly convex regions of non-strictly convex shapes + // like the ends of capsules. + contact_patch.addPoint(contact.pos); + return; + } + + // Step 2 - Compute support set of each shape, in the direction of + // the contact's normal. + // The first shape's support set is called "current"; it will be the first + // iterate of the Sutherland-Hodgman algorithm. The second shape's support set + // is called "clipper"; it will be used to clip "current". The support set + // computation step computes a convex polygon; its vertices are ordered + // counter-clockwise. This is important as the Sutherland-Hodgman algorithm + // expects points to be ranked counter-clockwise. + this->reset(s1, tf1, s2, tf2, contact_patch); + assert(this->num_samples_curved_shapes > 3); + + this->supportFuncShape1(&s1, this->support_set_shape1, this->support_guess[0], + this->supports_data[0], + this->num_samples_curved_shapes, + this->patch_tolerance); + + this->supportFuncShape2(&s2, this->support_set_shape2, this->support_guess[1], + this->supports_data[1], + this->num_samples_curved_shapes, + this->patch_tolerance); + + // We can immediatly return if one of the support set has only + // one point. + if (this->support_set_shape1.size() <= 1 || + this->support_set_shape2.size() <= 1) { + contact_patch.addPoint(contact.pos); + return; + } + + // `eps` is be used to check strict positivity of determinants. + const CoalScalar eps = Eigen::NumTraits::dummy_precision(); + using Polygon = SupportSet::Polygon; + + if ((this->support_set_shape1.size() == 2) && + (this->support_set_shape2.size() == 2)) { + // Segment-Segment case + // We compute the determinant; if it is non-zero, the intersection + // has already been computed: it's `Contact::pos`. + const Polygon& pts1 = this->support_set_shape1.points(); + const Vec2s& a = pts1[0]; + const Vec2s& b = pts1[1]; + + const Polygon& pts2 = this->support_set_shape2.points(); + const Vec2s& c = pts2[0]; + const Vec2s& d = pts2[1]; + + const CoalScalar det = + (b(0) - a(0)) * (d(1) - c(1)) >= (b(1) - a(1)) * (d(0) - c(0)); + if ((std::abs(det) > eps) || ((c - d).squaredNorm() < eps) || + ((b - a).squaredNorm() < eps)) { + contact_patch.addPoint(contact.pos); + return; + } + + const Vec2s cd = (d - c); + const CoalScalar l = cd.squaredNorm(); + Polygon& patch = contact_patch.points(); + + // Project a onto [c, d] + CoalScalar t1 = (a - c).dot(cd); + t1 = (t1 >= l) ? 1.0 : ((t1 <= 0) ? 0.0 : (t1 / l)); + const Vec2s p1 = c + t1 * cd; + patch.emplace_back(p1); + + // Project b onto [c, d] + CoalScalar t2 = (b - c).dot(cd); + t2 = (t2 >= l) ? 1.0 : ((t2 <= 0) ? 0.0 : (t2 / l)); + const Vec2s p2 = c + t2 * cd; + if ((p1 - p2).squaredNorm() >= eps) { + patch.emplace_back(p2); + } + return; + } + + // + // Step 3 - Main loop of the algorithm: use the "clipper" polygon to clip the + // "current" polygon. The resulting intersection is the contact patch of the + // contact between s1 and s2. "clipper" and "current" are the support sets of + // shape1 and shape2 (they can be swapped, i.e. clipper can be assigned to + // shape1 and current to shape2, depending on which case we are). Currently, + // to clip one polygon with the other, we use the Sutherland-Hodgman + // algorithm: + // https://en.wikipedia.org/wiki/Sutherland%E2%80%93Hodgman_algorithm + // In the general case, Sutherland-Hodgman clips one polygon of size >=3 using + // another polygon of size >=3. However, it can be easily extended to handle + // the segment-polygon case. + // + // The maximum size of the output of the Sutherland-Hodgman algorithm is n1 + + // n2 where n1 and n2 are the sizes of the first and second polygon. + const size_t max_result_size = + this->support_set_shape1.size() + this->support_set_shape2.size(); + if (this->added_to_patch.size() < max_result_size) { + this->added_to_patch.assign(max_result_size, false); + } + + const Polygon* clipper_ptr = nullptr; + Polygon* current_ptr = nullptr; + Polygon* previous_ptr = &(this->support_set_buffer.points()); + + // Let the clipper set be the one with the most vertices, to make sure it is + // at least a triangle. + if (this->support_set_shape1.size() < this->support_set_shape2.size()) { + current_ptr = &(this->support_set_shape1.points()); + clipper_ptr = &(this->support_set_shape2.points()); + } else { + current_ptr = &(this->support_set_shape2.points()); + clipper_ptr = &(this->support_set_shape1.points()); + } + + const Polygon& clipper = *(clipper_ptr); + const size_t clipper_size = clipper.size(); + for (size_t i = 0; i < clipper_size; ++i) { + // Swap `current` and `previous`. + // `previous` tracks the last iteration of the algorithm; `current` is + // filled by clipping `current` using `clipper`. + Polygon* tmp_ptr = previous_ptr; + previous_ptr = current_ptr; + current_ptr = tmp_ptr; + + const Polygon& previous = *(previous_ptr); + Polygon& current = *(current_ptr); + current.clear(); + + const Vec2s& a = clipper[i]; + const Vec2s& b = clipper[(i + 1) % clipper_size]; + const Vec2s ab = b - a; + + if (previous.size() == 2) { + // + // Segment-Polygon case + // + const Vec2s& p1 = previous[0]; + const Vec2s& p2 = previous[1]; + + const Vec2s ap1 = p1 - a; + const Vec2s ap2 = p2 - a; + + const CoalScalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0); + const CoalScalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0); + + if (det1 < 0 && det2 < 0) { + // Both p1 and p2 are outside the clipping polygon, i.e. there is no + // intersection. The algorithm can stop. + break; + } + + if (det1 >= 0 && det2 >= 0) { + // Both p1 and p2 are inside the clipping polygon, there is nothing to + // do; move to the next iteration. + current = previous; + continue; + } + + // Compute the intersection between the line (a, b) and the segment + // [p1, p2]. + if (det1 >= 0) { + if (det1 > eps) { + const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2); + current.emplace_back(p1); + current.emplace_back(p); + continue; + } else { + // p1 is the only point of current which is also a point of the + // clipper. We can exit. + current.emplace_back(p1); + break; + } + } else { + if (det2 > eps) { + const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2); + current.emplace_back(p2); + current.emplace_back(p); + continue; + } else { + // p2 is the only point of current which is also a point of the + // clipper. We can exit. + current.emplace_back(p2); + break; + } + } + } else { + // + // Polygon-Polygon case. + // + std::fill(this->added_to_patch.begin(), // + this->added_to_patch.end(), // + false); + + const size_t previous_size = previous.size(); + for (size_t j = 0; j < previous_size; ++j) { + const Vec2s& p1 = previous[j]; + const Vec2s& p2 = previous[(j + 1) % previous_size]; + + const Vec2s ap1 = p1 - a; + const Vec2s ap2 = p2 - a; + + const CoalScalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0); + const CoalScalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0); + + if (det1 < 0 && det2 < 0) { + // No intersection. Continue to next segment of previous. + continue; + } + + if (det1 >= 0 && det2 >= 0) { + // Both p1 and p2 are inside the clipping polygon, add p1 to current + // (only if it has not already been added). + if (!this->added_to_patch[j]) { + current.emplace_back(p1); + this->added_to_patch[j] = true; + } + // Continue to next segment of previous. + continue; + } + + if (det1 >= 0) { + if (det1 > eps) { + if (!this->added_to_patch[j]) { + current.emplace_back(p1); + this->added_to_patch[j] = true; + } + const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2); + current.emplace_back(p); + } else { + // a, b and p1 are colinear; we add only p1. + if (!this->added_to_patch[j]) { + current.emplace_back(p1); + this->added_to_patch[j] = true; + } + } + } else { + if (det2 > eps) { + const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2); + current.emplace_back(p); + } else { + if (!this->added_to_patch[(j + 1) % previous.size()]) { + current.emplace_back(p2); + this->added_to_patch[(j + 1) % previous.size()] = true; + } + } + } + } + } + // + // End of iteration i of Sutherland-Hodgman. + if (current.size() <= 1) { + // No intersection or one point found, the algo can early stop. + break; + } + } + + // Transfer the result of the Sutherland-Hodgman algorithm to the contact + // patch. + this->getResult(contact, current_ptr, contact_patch); +} + +// ============================================================================ +inline void ContactPatchSolver::getResult( + const Contact& contact, const ContactPatch::Polygon* result_ptr, + ContactPatch& contact_patch) const { + if (result_ptr->size() <= 1) { + contact_patch.addPoint(contact.pos); + return; + } + + const ContactPatch::Polygon& result = *(result_ptr); + ContactPatch::Polygon& patch = contact_patch.points(); + patch = result; +} + +// ============================================================================ +template +inline void ContactPatchSolver::reset(const ShapeType1& shape1, + const Transform3s& tf1, + const ShapeType2& shape2, + const Transform3s& tf2, + const ContactPatch& contact_patch) const { + // Reset internal quantities + this->support_set_shape1.clear(); + this->support_set_shape2.clear(); + this->support_set_buffer.clear(); + + // Get the support function of each shape + const Transform3s& tfc = contact_patch.tf; + + this->support_set_shape1.direction = SupportSetDirection::DEFAULT; + // Set the reference frame of the support set of the first shape to be the + // local frame of shape 1. + Transform3s& tf1c = this->support_set_shape1.tf; + tf1c.rotation().noalias() = tf1.rotation().transpose() * tfc.rotation(); + tf1c.translation().noalias() = + tf1.rotation().transpose() * (tfc.translation() - tf1.translation()); + this->supportFuncShape1 = + this->makeSupportSetFunction(&shape1, this->supports_data[0]); + + this->support_set_shape2.direction = SupportSetDirection::INVERTED; + // Set the reference frame of the support set of the second shape to be the + // local frame of shape 2. + Transform3s& tf2c = this->support_set_shape2.tf; + tf2c.rotation().noalias() = tf2.rotation().transpose() * tfc.rotation(); + tf2c.translation().noalias() = + tf2.rotation().transpose() * (tfc.translation() - tf2.translation()); + this->supportFuncShape2 = + this->makeSupportSetFunction(&shape2, this->supports_data[1]); +} + +// ========================================================================== +inline Vec2s ContactPatchSolver::computeLineSegmentIntersection( + const Vec2s& a, const Vec2s& b, const Vec2s& c, const Vec2s& d) { + const Vec2s ab = b - a; + const Vec2s n(-ab(1), ab(0)); + const CoalScalar denominator = n.dot(c - d); + if (std::abs(denominator) < std::numeric_limits::epsilon()) { + return d; + } + const CoalScalar nominator = n.dot(a - d); + CoalScalar alpha = nominator / denominator; + alpha = std::min(1.0, std::max(0.0, alpha)); + return alpha * c + (1 - alpha) * d; +} + +} // namespace coal + +#endif // COAL_CONTACT_PATCH_SOLVER_HXX diff --git a/include/coal/contact_patch_func_matrix.h b/include/coal/contact_patch_func_matrix.h new file mode 100644 index 000000000..f95d73c4e --- /dev/null +++ b/include/coal/contact_patch_func_matrix.h @@ -0,0 +1,85 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of INRIA nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Louis Montaut */ + +#ifndef COAL_CONTACT_PATCH_FUNC_MATRIX_H +#define COAL_CONTACT_PATCH_FUNC_MATRIX_H + +#include "coal/collision_data.h" +#include "coal/contact_patch/contact_patch_solver.h" +#include "coal/narrowphase/narrowphase.h" + +namespace coal { + +/// @brief The contact patch matrix stores the functions for contact patches +/// computation between different types of objects and provides a uniform call +/// interface +struct COAL_DLLAPI ContactPatchFunctionMatrix { + /// @brief the uniform call interface for computing contact patches: we need + /// know + /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 + /// and tf2; + /// 2. the collision result that generated contact patches candidates + /// (`coal::Contact`), from which contact patches will be expanded; + /// 3. the solver for computation of contact patches; + /// 4. the request setting for contact patches (e.g. maximum amount of + /// patches, patch tolerance etc.) + /// 5. the structure to return contact patches + /// (`coal::ContactPatchResult`). + /// + /// Note: we pass a GJKSolver, because it allows to reuse internal computation + /// that was made during the narrow phase. It also allows to experiment with + /// different ways to compute contact patches. We could, for example, perturb + /// tf1 and tf2 and make multiple calls to the GJKSolver (although this is not + /// the approach done by default). + typedef void (*ContactPatchFunc)(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, + const CollisionResult& collision_result, + const ContactPatchSolver* csolver, + const ContactPatchRequest& request, + ContactPatchResult& result); + + /// @brief Each item in the contact patch matrix is a function to handle + /// contact patch computation between objects of type1 and type2. + ContactPatchFunc contact_patch_matrix[NODE_COUNT][NODE_COUNT]; + + ContactPatchFunctionMatrix(); +}; + +} // namespace coal + +#endif diff --git a/include/coal/data_types.h b/include/coal/data_types.h new file mode 100644 index 000000000..11d34a981 --- /dev/null +++ b/include/coal/data_types.h @@ -0,0 +1,197 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2023, Inria + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_DATA_TYPES_H +#define COAL_DATA_TYPES_H + +#include +#include + +#include "coal/config.hh" + +#ifdef COAL_HAS_OCTOMAP +#define OCTOMAP_VERSION_AT_LEAST(x, y, z) \ + (OCTOMAP_MAJOR_VERSION > x || \ + (OCTOMAP_MAJOR_VERSION >= x && \ + (OCTOMAP_MINOR_VERSION > y || \ + (OCTOMAP_MINOR_VERSION >= y && OCTOMAP_PATCH_VERSION >= z)))) + +#define OCTOMAP_VERSION_AT_MOST(x, y, z) \ + (OCTOMAP_MAJOR_VERSION < x || \ + (OCTOMAP_MAJOR_VERSION <= x && \ + (OCTOMAP_MINOR_VERSION < y || \ + (OCTOMAP_MINOR_VERSION <= y && OCTOMAP_PATCH_VERSION <= z)))) +#endif // COAL_HAS_OCTOMAP + +namespace coal { +#ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL +// We keep the FCL_REAL typedef and the Vec[..]f typedefs for backward +// compatibility. +typedef double FCL_REAL; +typedef Eigen::Matrix Vec3f; +typedef Eigen::Matrix Vec2f; +typedef Eigen::Matrix Vec6f; +typedef Eigen::Matrix VecXf; +typedef Eigen::Matrix Matrix3f; +typedef Eigen::Matrix Matrixx3f; +typedef Eigen::Matrix Matrixx2f; +typedef Eigen::Matrix MatrixXf; +#endif + +typedef double CoalScalar; +typedef Eigen::Matrix Vec3s; +typedef Eigen::Matrix Vec2s; +typedef Eigen::Matrix Vec6s; +typedef Eigen::Matrix VecXs; +typedef Eigen::Matrix Matrix3s; +typedef Eigen::Matrix MatrixX3s; +typedef Eigen::Matrix MatrixX2s; +typedef Eigen::Matrix + Matrixx3i; +typedef Eigen::Matrix MatrixXs; +typedef Eigen::Vector2i support_func_guess_t; + +/// @brief Initial guess to use for the GJK algorithm +/// DefaultGuess: Vec3s(1, 0, 0) +/// CachedGuess: previous vector found by GJK or guess cached by the user +/// BoundingVolumeGuess: guess using the centers of the shapes' AABB +/// WARNING: to use BoundingVolumeGuess, computeLocalAABB must have been called +/// on the two shapes. +enum GJKInitialGuess { DefaultGuess, CachedGuess, BoundingVolumeGuess }; + +/// @brief Variant to use for the GJK algorithm +enum GJKVariant { DefaultGJK, PolyakAcceleration, NesterovAcceleration }; + +/// @brief Which convergence criterion is used to stop the algorithm (when the +/// shapes are not in collision). (default) VDB: Van den Bergen (A Fast and +/// Robust GJK Implementation, 1999) DG: duality-gap, as used in the Frank-Wolfe +/// and the vanilla 1988 GJK algorithms Hybrid: a mix between VDB and DG. +enum GJKConvergenceCriterion { Default, DualityGap, Hybrid }; + +/// @brief Wether the convergence criterion is scaled on the norm of the +/// solution or not +enum GJKConvergenceCriterionType { Relative, Absolute }; + +/// @brief Triangle with 3 indices for points +class COAL_DLLAPI Triangle { + public: + typedef std::size_t index_type; + typedef int size_type; + + /// @brief Default constructor + Triangle() {} + + /// @brief Create a triangle with given vertex indices + Triangle(index_type p1, index_type p2, index_type p3) { set(p1, p2, p3); } + + /// @brief Set the vertex indices of the triangle + inline void set(index_type p1, index_type p2, index_type p3) { + vids[0] = p1; + vids[1] = p2; + vids[2] = p3; + } + + /// @brief Access the triangle index + inline index_type operator[](index_type i) const { return vids[i]; } + + inline index_type& operator[](index_type i) { return vids[i]; } + + static inline size_type size() { return 3; } + + bool operator==(const Triangle& other) const { + return vids[0] == other.vids[0] && vids[1] == other.vids[1] && + vids[2] == other.vids[2]; + } + + bool operator!=(const Triangle& other) const { return !(*this == other); } + + bool isValid() const { + return vids[0] != (std::numeric_limits::max)() && + vids[1] != (std::numeric_limits::max)() && + vids[2] != (std::numeric_limits::max)(); + } + + private: + /// @brief indices for each vertex of triangle + index_type vids[3] = {(std::numeric_limits::max)(), + (std::numeric_limits::max)(), + (std::numeric_limits::max)()}; +}; + +/// @brief Quadrilateral with 4 indices for points +struct COAL_DLLAPI Quadrilateral { + typedef std::size_t index_type; + typedef int size_type; + + Quadrilateral() {} + + Quadrilateral(index_type p0, index_type p1, index_type p2, index_type p3) { + set(p0, p1, p2, p3); + } + + /// @brief Set the vertex indices of the quadrilateral + inline void set(index_type p0, index_type p1, index_type p2, index_type p3) { + vids[0] = p0; + vids[1] = p1; + vids[2] = p2; + vids[3] = p3; + } + + /// @access the quadrilateral index + inline index_type operator[](index_type i) const { return vids[i]; } + + inline index_type& operator[](index_type i) { return vids[i]; } + + static inline size_type size() { return 4; } + + bool operator==(const Quadrilateral& other) const { + return vids[0] == other.vids[0] && vids[1] == other.vids[1] && + vids[2] == other.vids[2] && vids[3] == other.vids[3]; + } + + bool operator!=(const Quadrilateral& other) const { + return !(*this == other); + } + + private: + index_type vids[4]; +}; + +} // namespace coal + +#endif diff --git a/include/coal/distance.h b/include/coal/distance.h new file mode 100644 index 000000000..7dd4a3e7d --- /dev/null +++ b/include/coal/distance.h @@ -0,0 +1,115 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef COAL_DISTANCE_H +#define COAL_DISTANCE_H + +#include "coal/collision_object.h" +#include "coal/collision_data.h" +#include "coal/distance_func_matrix.h" +#include "coal/timings.h" + +namespace coal { + +/// @brief Main distance interface: given two collision objects, and the +/// requirements for contacts, including whether return the nearest points, this +/// function performs the distance between them. Return value is the minimum +/// distance generated between the two objects. +COAL_DLLAPI CoalScalar distance(const CollisionObject* o1, + const CollisionObject* o2, + const DistanceRequest& request, + DistanceResult& result); + +/// @copydoc distance(const CollisionObject*, const CollisionObject*, const +/// DistanceRequest&, DistanceResult&) +COAL_DLLAPI CoalScalar distance(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, + const DistanceRequest& request, + DistanceResult& result); + +/// This class reduces the cost of identifying the geometry pair. +/// This is mostly useful for repeated shape-shape queries. +/// +/// \code +/// ComputeDistance calc_distance (o1, o2); +/// CoalScalar distance = calc_distance(tf1, tf2, request, result); +/// \endcode +class COAL_DLLAPI ComputeDistance { + public: + ComputeDistance(const CollisionGeometry* o1, const CollisionGeometry* o2); + + CoalScalar operator()(const Transform3s& tf1, const Transform3s& tf2, + const DistanceRequest& request, + DistanceResult& result) const; + + bool operator==(const ComputeDistance& other) const { + return o1 == other.o1 && o2 == other.o2 && swap_geoms == other.swap_geoms && + solver == other.solver && func == other.func; + } + + bool operator!=(const ComputeDistance& other) const { + return !(*this == other); + } + + virtual ~ComputeDistance() {}; + + protected: + // These pointers are made mutable to let the derived classes to update + // their values when updating the collision geometry (e.g. creating a new + // one). This feature should be used carefully to avoid any mis usage (e.g, + // changing the type of the collision geometry should be avoided). + mutable const CollisionGeometry* o1; + mutable const CollisionGeometry* o2; + + mutable GJKSolver solver; + + DistanceFunctionMatrix::DistanceFunc func; + bool swap_geoms; + + virtual CoalScalar run(const Transform3s& tf1, const Transform3s& tf2, + const DistanceRequest& request, + DistanceResult& result) const; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace coal + +#endif diff --git a/include/coal/distance_func_matrix.h b/include/coal/distance_func_matrix.h new file mode 100644 index 000000000..a37e1df51 --- /dev/null +++ b/include/coal/distance_func_matrix.h @@ -0,0 +1,74 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_DISTANCE_FUNC_MATRIX_H +#define COAL_DISTANCE_FUNC_MATRIX_H + +#include "coal/collision_object.h" +#include "coal/collision_data.h" +#include "coal/narrowphase/narrowphase.h" + +namespace coal { + +/// @brief distance matrix stores the functions for distance between different +/// types of objects and provides a uniform call interface +struct COAL_DLLAPI DistanceFunctionMatrix { + /// @brief the uniform call interface for distance: for distance, we need know + /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 + /// and tf2; + /// 2. the solver for narrow phase collision, this is for distance computation + /// between geometric shapes; + /// 3. the request setting for distance (e.g., whether need to return nearest + /// points); + typedef CoalScalar (*DistanceFunc)(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, + const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result); + + /// @brief each item in the distance matrix is a function to handle distance + /// between objects of type1 and type2 + DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT]; + + DistanceFunctionMatrix(); +}; + +} // namespace coal + +#endif diff --git a/include/hpp/fcl/doc.hh b/include/coal/doc.hh similarity index 79% rename from include/hpp/fcl/doc.hh rename to include/coal/doc.hh index 4d645bf23..7b86b9d73 100644 --- a/include/hpp/fcl/doc.hh +++ b/include/coal/doc.hh @@ -32,30 +32,29 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -namespace hpp { -namespace fcl { +namespace coal { /// \mainpage -/// \anchor hpp_fcl_documentation +/// \anchor coal_documentation /// -/// \section hpp_fcl_introduction Introduction +/// \section coal_introduction Introduction /// -/// hpp-fcl is a modified version the FCL libraries. +/// Coal is a modified version the FCL libraries. /// /// It is a library for collision detection and distance computation between /// various types of geometric shapes reprensented either by -/// \li basic shapes (hpp::fcl::ShapeBase) like box, sphere, cylinders, ... -/// \li or by bounding volume hierarchies of various types (hpp::fcl::BVHModel) +/// \li basic shapes (coal::ShapeBase) like box, sphere, cylinders, ... +/// \li or by bounding volume hierarchies of various types (coal::BVHModel) /// -/// \par Using hpp-fcl +/// \par Using Coal /// /// The main entry points to the library are functions -/// \li hpp::fcl::collide(const CollisionObject*, const CollisionObject*, const -/// CollisionRequest&, CollisionResult&) \li hpp::fcl::distance(const +/// \li coal::collide(const CollisionObject*, const CollisionObject*, const +/// CollisionRequest&, CollisionResult&) \li coal::distance(const /// CollisionObject*, const CollisionObject*, const DistanceRequest&, /// DistanceResult&) /// -/// \section hpp_fcl_collision_and_distance_lower_bound_computation Collision +/// \section coal_collision_and_distance_lower_bound_computation Collision /// detection and distance lower bound /// /// Collision queries can return a distance lower bound between the two objects, @@ -72,9 +71,8 @@ namespace fcl { /// is only guaranted that it will be inferior to /// distance - security_margin and superior to \em break_distance. /// \note If CollisionRequest::security_margin is set to -inf, no collision test -/// is performed by function hpp::fcl::collide or class -/// hpp::fcl::ComputeCollision and the objects are considered as not +/// is performed by function coal::collide or class +/// coal::ComputeCollision and the objects are considered as not /// colliding. -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/include/coal/fwd.hh b/include/coal/fwd.hh new file mode 100644 index 000000000..c2408880a --- /dev/null +++ b/include/coal/fwd.hh @@ -0,0 +1,158 @@ +// +// Software License Agreement (BSD License) +// +// Copyright (c) 2014, CNRS-LAAS +// Copyright (c) 2022-2024, Inria +// Author: Florent Lamiraux +// +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of CNRS-LAAS nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// + +#ifndef COAL_FWD_HH +#define COAL_FWD_HH + +#include +#include +#include +#include + +#include "coal/config.hh" +#include "coal/deprecated.hh" +#include "coal/warning.hh" + +#if _WIN32 +#define COAL_PRETTY_FUNCTION __FUNCSIG__ +#else +#define COAL_PRETTY_FUNCTION __PRETTY_FUNCTION__ +#endif + +#define COAL_UNUSED_VARIABLE(var) (void)(var) + +#ifdef NDEBUG +#define COAL_ONLY_USED_FOR_DEBUG(var) COAL_UNUSED_VARIABLE(var) +#else +#define COAL_ONLY_USED_FOR_DEBUG(var) +#endif + +#define COAL_THROW_PRETTY(message, exception) \ + { \ + std::stringstream ss; \ + ss << "From file: " << __FILE__ << "\n"; \ + ss << "in function: " << COAL_PRETTY_FUNCTION << "\n"; \ + ss << "at line: " << __LINE__ << "\n"; \ + ss << "message: " << message << "\n"; \ + throw exception(ss.str()); \ + } + +#ifdef COAL_TURN_ASSERT_INTO_EXCEPTION +#define COAL_ASSERT(check, message, exception) \ + do { \ + if (!(check)) { \ + COAL_THROW_PRETTY(message, exception); \ + } \ + } while (0) +#else +#define COAL_ASSERT(check, message, exception) \ + { \ + COAL_UNUSED_VARIABLE(exception(message)); \ + assert((check) && message); \ + } +#endif + +#if (__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) +#define COAL_WITH_CXX11_SUPPORT +#endif + +#if defined(__GNUC__) || defined(__clang__) +#define COAL_COMPILER_DIAGNOSTIC_PUSH _Pragma("GCC diagnostic push") +#define COAL_COMPILER_DIAGNOSTIC_POP _Pragma("GCC diagnostic pop") +#define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \ + _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"") + +// GCC version 4.6 and higher supports -Wmaybe-uninitialized +#if (defined(__GNUC__) && \ + ((__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6))) +#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \ + _Pragma("GCC diagnostic ignored \"-Wmaybe-uninitialized\"") +// Use __has_warning with clang. Clang introduced it in 2024 (3.5+) +#elif (defined(__clang__) && defined(__has_warning) && \ + __has_warning("-Wmaybe-uninitialized")) +#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \ + _Pragma("clang diagnostic ignored \"-Wmaybe-uninitialized\"") +#else +#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED +#endif +#elif defined(WIN32) +#define COAL_COMPILER_DIAGNOSTIC_PUSH _Pragma("warning(push)") +#define COAL_COMPILER_DIAGNOSTIC_POP _Pragma("warning(pop)") +#define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \ + _Pragma("warning(disable : 4996)") +#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \ + _Pragma("warning(disable : 4700)") +#else +#define COAL_COMPILER_DIAGNOSTIC_PUSH +#define COAL_COMPILER_DIAGNOSTIC_POP +#define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS +#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED +#endif // __GNUC__ + +namespace coal { +using std::dynamic_pointer_cast; +using std::make_shared; +using std::shared_ptr; + +class CollisionObject; +typedef shared_ptr CollisionObjectPtr_t; +typedef shared_ptr CollisionObjectConstPtr_t; +class CollisionGeometry; +typedef shared_ptr CollisionGeometryPtr_t; +typedef shared_ptr CollisionGeometryConstPtr_t; +class Transform3s; + +class AABB; + +class BVHModelBase; +typedef shared_ptr BVHModelPtr_t; + +class OcTree; +typedef shared_ptr OcTreePtr_t; +typedef shared_ptr OcTreeConstPtr_t; +} // namespace coal + +#ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL +namespace hpp { +namespace fcl { +using namespace coal; +using Transform3f = Transform3s; // For backward compatibility +} // namespace fcl +} // namespace hpp +#endif + +#endif // COAL_FWD_HH diff --git a/include/coal/hfield.h b/include/coal/hfield.h new file mode 100644 index 000000000..9949296ed --- /dev/null +++ b/include/coal/hfield.h @@ -0,0 +1,543 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021-2024, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Justin Carpentier */ + +#ifndef COAL_HEIGHT_FIELD_H +#define COAL_HEIGHT_FIELD_H + +#include "coal/fwd.hh" +#include "coal/data_types.h" +#include "coal/collision_object.h" +#include "coal/BV/BV_node.h" +#include "coal/BVH/BVH_internal.h" + +#include + +namespace coal { + +/// @addtogroup Construction_Of_HeightField +/// @{ + +struct COAL_DLLAPI HFNodeBase { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum class FaceOrientation { + TOP = 1, + BOTTOM = 1, + NORTH = 2, + EAST = 4, + SOUTH = 8, + WEST = 16 + }; + + /// @brief An index for first child node or primitive + /// If the value is positive, it is the index of the first child bv node + /// If the value is negative, it is -(primitive index + 1) + /// Zero is not used. + size_t first_child; + + Eigen::DenseIndex x_id, x_size; + Eigen::DenseIndex y_id, y_size; + + CoalScalar max_height; + int contact_active_faces; + + /// @brief Default constructor + HFNodeBase() + : first_child(0), + x_id(-1), + x_size(0), + y_id(-1), + y_size(0), + max_height(std::numeric_limits::lowest()), + contact_active_faces(0) {} + + /// @brief Comparison operator + bool operator==(const HFNodeBase& other) const { + return first_child == other.first_child && x_id == other.x_id && + x_size == other.x_size && y_id == other.y_id && + y_size == other.y_size && max_height == other.max_height && + contact_active_faces == other.contact_active_faces; + } + + /// @brief Difference operator + bool operator!=(const HFNodeBase& other) const { return !(*this == other); } + + /// @brief Whether current node is a leaf node (i.e. contains a primitive + /// index) + inline bool isLeaf() const { return x_size == 1 && y_size == 1; } + + /// @brief Return the index of the first child. The index is referred to the + /// bounding volume array (i.e. bvs) in BVHModel + inline size_t leftChild() const { return first_child; } + + /// @brief Return the index of the second child. The index is referred to the + /// bounding volume array (i.e. bvs) in BVHModel + inline size_t rightChild() const { return first_child + 1; } + + inline Eigen::Vector2i leftChildIndexes() const { + return Eigen::Vector2i(x_id, y_id); + } + inline Eigen::Vector2i rightChildIndexes() const { + return Eigen::Vector2i(x_id + x_size / 2, y_id + y_size / 2); + } +}; + +inline HFNodeBase::FaceOrientation operator&(HFNodeBase::FaceOrientation a, + HFNodeBase::FaceOrientation b) { + return HFNodeBase::FaceOrientation(int(a) & int(b)); +} + +inline int operator&(int a, HFNodeBase::FaceOrientation b) { + return a & int(b); +} + +template +struct COAL_DLLAPI HFNode : public HFNodeBase { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef HFNodeBase Base; + + /// @brief bounding volume storing the geometry + BV bv; + + /// @brief Equality operator + bool operator==(const HFNode& other) const { + return Base::operator==(other) && bv == other.bv; + } + + /// @brief Difference operator + bool operator!=(const HFNode& other) const { return !(*this == other); } + + /// @brief Check whether two BVNode collide + bool overlap(const HFNode& other) const { return bv.overlap(other.bv); } + /// @brief Check whether two BVNode collide + bool overlap(const HFNode& other, const CollisionRequest& request, + CoalScalar& sqrDistLowerBound) const { + return bv.overlap(other.bv, request, sqrDistLowerBound); + } + + /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and + /// the underlying BV supports distance, return the nearest points. + CoalScalar distance(const HFNode& other, Vec3s* P1 = NULL, + Vec3s* P2 = NULL) const { + return bv.distance(other.bv, P1, P2); + } + + /// @brief Access to the center of the BV + Vec3s getCenter() const { return bv.center(); } + + /// @brief Access to the orientation of the BV + coal::Matrix3s::IdentityReturnType getOrientation() const { + return Matrix3s::Identity(); + } + + virtual ~HFNode() {} +}; + +namespace details { + +template +struct UpdateBoundingVolume { + static void run(const Vec3s& pointA, const Vec3s& pointB, BV& bv) { + AABB bv_aabb(pointA, pointB); + // AABB bv_aabb; + // bv_aabb.update(pointA,pointB); + convertBV(bv_aabb, bv); + } +}; + +template <> +struct UpdateBoundingVolume { + static void run(const Vec3s& pointA, const Vec3s& pointB, AABB& bv) { + AABB bv_aabb(pointA, pointB); + convertBV(bv_aabb, bv); + // bv.update(pointA,pointB); + } +}; + +} // namespace details + +/// @brief Data structure depicting a height field given by the base grid +/// dimensions and the elevation along the grid. \tparam BV one of the bounding +/// volume class in \ref Bounding_Volume. +/// +/// An height field is defined by its base dimensions along the X and Y axes and +/// a set ofpoints defined by their altitude, regularly dispatched on the grid. +/// The height field is centered at the origin and the corners of the geometry +/// correspond to the following coordinates [± x_dim/2; ± y_dim/2]. +template +class COAL_DLLAPI HeightField : public CollisionGeometry { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef CollisionGeometry Base; + + typedef HFNode Node; + typedef std::vector> BVS; + + /// @brief Constructing an empty HeightField + HeightField() + : CollisionGeometry(), + min_height((std::numeric_limits::min)()), + max_height((std::numeric_limits::max)()) {} + + /// @brief Constructing an HeightField from its base dimensions and the set of + /// heights points. + /// The granularity of the height field along X and Y direction is + /// extraded from the Z grid. + /// + /// \param[in] x_dim Dimension along the X axis + /// \param[in] y_dim Dimension along the Y axis + /// \param[in] heights Matrix containing the altitude of each point compositng + /// the height field + /// \param[in] min_height Minimal height of the height field + /// + HeightField(const CoalScalar x_dim, const CoalScalar y_dim, + const MatrixXs& heights, + const CoalScalar min_height = (CoalScalar)0) + : CollisionGeometry() { + init(x_dim, y_dim, heights, min_height); + } + + /// @brief Copy contructor from another HeightField + /// + /// \param[in] other to copy. + /// + HeightField(const HeightField& other) + : CollisionGeometry(other), + x_dim(other.x_dim), + y_dim(other.y_dim), + heights(other.heights), + min_height(other.min_height), + max_height(other.max_height), + x_grid(other.x_grid), + y_grid(other.y_grid), + bvs(other.bvs), + num_bvs(other.num_bvs) {} + + /// @brief Returns a const reference of the grid along the X direction. + const VecXs& getXGrid() const { return x_grid; } + /// @brief Returns a const reference of the grid along the Y direction. + const VecXs& getYGrid() const { return y_grid; } + + /// @brief Returns a const reference of the heights + const MatrixXs& getHeights() const { return heights; } + + /// @brief Returns the dimension of the Height Field along the X direction. + CoalScalar getXDim() const { return x_dim; } + /// @brief Returns the dimension of the Height Field along the Y direction. + CoalScalar getYDim() const { return y_dim; } + + /// @brief Returns the minimal height value of the Height Field. + CoalScalar getMinHeight() const { return min_height; } + /// @brief Returns the maximal height value of the Height Field. + CoalScalar getMaxHeight() const { return max_height; } + + virtual HeightField* clone() const { return new HeightField(*this); } + + const BVS& getNodes() const { return bvs; } + + /// @brief deconstruction, delete mesh data related. + virtual ~HeightField() {} + + /// @brief Compute the AABB for the HeightField, used for broad-phase + /// collision + void computeLocalAABB() { + const Vec3s A(x_grid[0], y_grid[0], min_height); + const Vec3s B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1], + max_height); + const AABB aabb_(A, B); + + aabb_radius = (A - B).norm() / 2.; + aabb_local = aabb_; + aabb_center = aabb_.center(); + } + + /// @brief Update Height Field height + void updateHeights(const MatrixXs& new_heights) { + if (new_heights.rows() != heights.rows() || + new_heights.cols() != heights.cols()) + COAL_THROW_PRETTY( + "The matrix containing the new heights values does not have the same " + "matrix size as the original one.\n" + "\tinput values - rows: " + << new_heights.rows() << " - cols: " << new_heights.cols() << "\n" + << "\texpected values - rows: " << heights.rows() + << " - cols: " << heights.cols() << "\n", + std::invalid_argument); + + heights = new_heights.cwiseMax(min_height); + this->max_height = recursiveUpdateHeight(0); + assert(this->max_height == heights.maxCoeff()); + } + + protected: + void init(const CoalScalar x_dim, const CoalScalar y_dim, + const MatrixXs& heights, const CoalScalar min_height) { + this->x_dim = x_dim; + this->y_dim = y_dim; + this->heights = heights.cwiseMax(min_height); + this->min_height = min_height; + this->max_height = heights.maxCoeff(); + + const Eigen::DenseIndex NX = heights.cols(), NY = heights.rows(); + assert(NX >= 2 && "The number of columns is too small."); + assert(NY >= 2 && "The number of rows is too small."); + + x_grid = VecXs::LinSpaced(NX, -0.5 * x_dim, 0.5 * x_dim); + y_grid = VecXs::LinSpaced(NY, 0.5 * y_dim, -0.5 * y_dim); + + // Allocate BVS + const size_t num_tot_bvs = + (size_t)(NX * NY) - 1 + (size_t)((NX - 1) * (NY - 1)); + bvs.resize(num_tot_bvs); + num_bvs = 0; + + // Build tree + buildTree(); + } + + /// @brief Get the object type: it is a HFIELD + OBJECT_TYPE getObjectType() const { return OT_HFIELD; } + + Vec3s computeCOM() const { return Vec3s::Zero(); } + + CoalScalar computeVolume() const { return 0; } + + Matrix3s computeMomentofInertia() const { return Matrix3s::Zero(); } + + protected: + /// @brief Dimensions in meters along X and Y directions + CoalScalar x_dim, y_dim; + + /// @brief Elevation values in meters of the Height Field + MatrixXs heights; + + /// @brief Minimal height of the Height Field: all values bellow min_height + /// will be discarded. + CoalScalar min_height, max_height; + + /// @brief Grids along the X and Y directions. Useful for plotting or other + /// related things. + VecXs x_grid, y_grid; + + /// @brief Bounding volume hierarchy + BVS bvs; + unsigned int num_bvs; + + /// @brief Build the bounding volume hierarchy + int buildTree() { + num_bvs = 1; + const CoalScalar max_recursive_height = + recursiveBuildTree(0, 0, heights.cols() - 1, 0, heights.rows() - 1); + assert(max_recursive_height == max_height && + "the maximal height is not correct"); + COAL_UNUSED_VARIABLE(max_recursive_height); + + bvs.resize(num_bvs); + return BVH_OK; + } + + CoalScalar recursiveUpdateHeight(const size_t bv_id) { + HFNode& bv_node = bvs[bv_id]; + + CoalScalar max_height; + if (bv_node.isLeaf()) { + max_height = heights.block<2, 2>(bv_node.y_id, bv_node.x_id).maxCoeff(); + } else { + CoalScalar max_left_height = recursiveUpdateHeight(bv_node.leftChild()), + max_right_height = recursiveUpdateHeight(bv_node.rightChild()); + + max_height = (std::max)(max_left_height, max_right_height); + } + + bv_node.max_height = max_height; + + const Vec3s pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height); + const Vec3s pointB(x_grid[bv_node.x_id + bv_node.x_size], + y_grid[bv_node.y_id + bv_node.y_size], max_height); + + details::UpdateBoundingVolume::run(pointA, pointB, bv_node.bv); + + return max_height; + } + + CoalScalar recursiveBuildTree(const size_t bv_id, + const Eigen::DenseIndex x_id, + const Eigen::DenseIndex x_size, + const Eigen::DenseIndex y_id, + const Eigen::DenseIndex y_size) { + assert(x_id < heights.cols() && "x_id is out of bounds"); + assert(y_id < heights.rows() && "y_id is out of bounds"); + assert(x_size >= 0 && y_size >= 0 && + "x_size or y_size are not of correct value"); + assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension"); + + HFNode& bv_node = bvs[bv_id]; + CoalScalar max_height; + if (x_size == 1 && + y_size == 1) // don't build any BV for the current child node + { + max_height = heights.block<2, 2>(y_id, x_id).maxCoeff(); + } else { + bv_node.first_child = num_bvs; + num_bvs += 2; + + CoalScalar max_left_height = min_height, max_right_height = min_height; + if (x_size >= y_size) // splitting along the X axis + { + Eigen::DenseIndex x_size_half = x_size / 2; + if (x_size == 1) x_size_half = 1; + max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, + x_size_half, y_id, y_size); + + max_right_height = + recursiveBuildTree(bv_node.rightChild(), x_id + x_size_half, + x_size - x_size_half, y_id, y_size); + } else // splitting along the Y axis + { + Eigen::DenseIndex y_size_half = y_size / 2; + if (y_size == 1) y_size_half = 1; + max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size, + y_id, y_size_half); + + max_right_height = + recursiveBuildTree(bv_node.rightChild(), x_id, x_size, + y_id + y_size_half, y_size - y_size_half); + } + + max_height = (std::max)(max_left_height, max_right_height); + } + + bv_node.max_height = max_height; + // max_height = std::max(max_height,min_height); + + const Vec3s pointA(x_grid[x_id], y_grid[y_id], min_height); + assert(x_id + x_size < x_grid.size()); + assert(y_id + y_size < y_grid.size()); + const Vec3s pointB(x_grid[x_id + x_size], y_grid[y_id + y_size], + max_height); + + details::UpdateBoundingVolume::run(pointA, pointB, bv_node.bv); + bv_node.x_id = x_id; + bv_node.y_id = y_id; + bv_node.x_size = x_size; + bv_node.y_size = y_size; + + if (bv_node.isLeaf()) { + int& contact_active_faces = bv_node.contact_active_faces; + contact_active_faces |= int(HFNodeBase::FaceOrientation::TOP); + contact_active_faces |= int(HFNodeBase::FaceOrientation::BOTTOM); + + if (bv_node.x_id == 0) // first col + contact_active_faces |= int(HFNodeBase::FaceOrientation::WEST); + + if (bv_node.y_id == 0) // first row (TOP) + contact_active_faces |= int(HFNodeBase::FaceOrientation::NORTH); + + if (bv_node.x_id + 1 == heights.cols() - 1) // last col + contact_active_faces |= int(HFNodeBase::FaceOrientation::EAST); + + if (bv_node.y_id + 1 == heights.rows() - 1) // last row (BOTTOM) + contact_active_faces |= int(HFNodeBase::FaceOrientation::SOUTH); + } + + return max_height; + } + + public: + /// @brief Access the bv giving the its index + const HFNode& getBV(unsigned int i) const { + if (i >= num_bvs) + COAL_THROW_PRETTY("Index out of bounds", std::invalid_argument); + return bvs[i]; + } + + /// @brief Access the bv giving the its index + HFNode& getBV(unsigned int i) { + if (i >= num_bvs) + COAL_THROW_PRETTY("Index out of bounds", std::invalid_argument); + return bvs[i]; + } + + /// @brief Get the BV type: default is unknown + NODE_TYPE getNodeType() const { return BV_UNKNOWN; } + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const HeightField* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const HeightField& other = *other_ptr; + + return x_dim == other.x_dim && y_dim == other.y_dim && + heights == other.heights && min_height == other.min_height && + max_height == other.max_height && x_grid == other.x_grid && + y_grid == other.y_grid && bvs == other.bvs && + num_bvs == other.num_bvs; + } +}; + +/// @brief Specialization of getNodeType() for HeightField with different BV +/// types +template <> +NODE_TYPE HeightField::getNodeType() const; + +template <> +NODE_TYPE HeightField::getNodeType() const; + +template <> +NODE_TYPE HeightField::getNodeType() const; + +template <> +NODE_TYPE HeightField::getNodeType() const; + +template <> +NODE_TYPE HeightField::getNodeType() const; + +template <> +NODE_TYPE HeightField>::getNodeType() const; + +template <> +NODE_TYPE HeightField>::getNodeType() const; + +template <> +NODE_TYPE HeightField>::getNodeType() const; + +/// @} + +} // namespace coal + +#endif diff --git a/include/coal/internal/BV_fitter.h b/include/coal/internal/BV_fitter.h new file mode 100644 index 000000000..a115d914f --- /dev/null +++ b/include/coal/internal/BV_fitter.h @@ -0,0 +1,220 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_BV_FITTER_H +#define COAL_BV_FITTER_H + +#include "coal/BVH/BVH_internal.h" +#include "coal/BV/kIOS.h" +#include "coal/BV/OBBRSS.h" +#include "coal/BV/AABB.h" +#include + +namespace coal { + +/// @brief Compute a bounding volume that fits a set of n points. +template +void fit(Vec3s* ps, unsigned int n, BV& bv) { + for (unsigned int i = 0; i < n; ++i) // TODO(jcarpent): vectorize + { + bv += ps[i]; + } +} + +template <> +void fit(Vec3s* ps, unsigned int n, OBB& bv); + +template <> +void fit(Vec3s* ps, unsigned int n, RSS& bv); + +template <> +void fit(Vec3s* ps, unsigned int n, kIOS& bv); + +template <> +void fit(Vec3s* ps, unsigned int n, OBBRSS& bv); + +template <> +void fit(Vec3s* ps, unsigned int n, AABB& bv); + +/// @brief The class for the default algorithm fitting a bounding volume to a +/// set of points +template +class COAL_DLLAPI BVFitterTpl { + public: + /// @brief default deconstructor + virtual ~BVFitterTpl() {} + + /// @brief Prepare the geometry primitive data for fitting + void set(Vec3s* vertices_, Triangle* tri_indices_, BVHModelType type_) { + vertices = vertices_; + prev_vertices = NULL; + tri_indices = tri_indices_; + type = type_; + } + + /// @brief Prepare the geometry primitive data for fitting, for deformable + /// mesh + void set(Vec3s* vertices_, Vec3s* prev_vertices_, Triangle* tri_indices_, + BVHModelType type_) { + vertices = vertices_; + prev_vertices = prev_vertices_; + tri_indices = tri_indices_; + type = type_; + } + + /// @brief Compute the fitting BV + virtual BV fit(unsigned int* primitive_indices, + unsigned int num_primitives) = 0; + + /// @brief Clear the geometry primitive data + void clear() { + vertices = NULL; + prev_vertices = NULL; + tri_indices = NULL; + type = BVH_MODEL_UNKNOWN; + } + + protected: + Vec3s* vertices; + Vec3s* prev_vertices; + Triangle* tri_indices; + BVHModelType type; +}; + +/// @brief The class for the default algorithm fitting a bounding volume to a +/// set of points +template +class COAL_DLLAPI BVFitter : public BVFitterTpl { + typedef BVFitterTpl Base; + + public: + /// @brief Compute a bounding volume that fits a set of primitives (points or + /// triangles). The primitive data was set by set function and + /// primitive_indices is the primitive index relative to the data + BV fit(unsigned int* primitive_indices, unsigned int num_primitives) { + BV bv; + + if (type == BVH_MODEL_TRIANGLES) /// The primitive is triangle + { + for (unsigned int i = 0; i < num_primitives; ++i) { + Triangle t = tri_indices[primitive_indices[i]]; + bv += vertices[t[0]]; + bv += vertices[t[1]]; + bv += vertices[t[2]]; + + if (prev_vertices) /// can fitting both current and previous frame + { + bv += prev_vertices[t[0]]; + bv += prev_vertices[t[1]]; + bv += prev_vertices[t[2]]; + } + } + } else if (type == BVH_MODEL_POINTCLOUD) /// The primitive is point + { + for (unsigned int i = 0; i < num_primitives; ++i) { + bv += vertices[primitive_indices[i]]; + + if (prev_vertices) /// can fitting both current and previous frame + { + bv += prev_vertices[primitive_indices[i]]; + } + } + } + + return bv; + } + + protected: + using Base::prev_vertices; + using Base::tri_indices; + using Base::type; + using Base::vertices; +}; + +/// @brief Specification of BVFitter for OBB bounding volume +template <> +class COAL_DLLAPI BVFitter : public BVFitterTpl { + public: + /// @brief Compute a bounding volume that fits a set of primitives (points or + /// triangles). The primitive data was set by set function and + /// primitive_indices is the primitive index relative to the data. + OBB fit(unsigned int* primitive_indices, unsigned int num_primitives); +}; + +/// @brief Specification of BVFitter for RSS bounding volume +template <> +class COAL_DLLAPI BVFitter : public BVFitterTpl { + public: + /// @brief Compute a bounding volume that fits a set of primitives (points or + /// triangles). The primitive data was set by set function and + /// primitive_indices is the primitive index relative to the data. + RSS fit(unsigned int* primitive_indices, unsigned int num_primitives); +}; + +/// @brief Specification of BVFitter for kIOS bounding volume +template <> +class COAL_DLLAPI BVFitter : public BVFitterTpl { + public: + /// @brief Compute a bounding volume that fits a set of primitives (points or + /// triangles). The primitive data was set by set function and + /// primitive_indices is the primitive index relative to the data. + kIOS fit(unsigned int* primitive_indices, unsigned int num_primitives); +}; + +/// @brief Specification of BVFitter for OBBRSS bounding volume +template <> +class COAL_DLLAPI BVFitter : public BVFitterTpl { + public: + /// @brief Compute a bounding volume that fits a set of primitives (points or + /// triangles). The primitive data was set by set function and + /// primitive_indices is the primitive index relative to the data. + OBBRSS fit(unsigned int* primitive_indices, unsigned int num_primitives); +}; + +/// @brief Specification of BVFitter for AABB bounding volume +template <> +class COAL_DLLAPI BVFitter : public BVFitterTpl { + public: + /// @brief Compute a bounding volume that fits a set of primitives (points or + /// triangles). The primitive data was set by set function and + /// primitive_indices is the primitive index relative to the data. + AABB fit(unsigned int* primitive_indices, unsigned int num_primitives); +}; + +} // namespace coal + +#endif diff --git a/include/coal/internal/BV_splitter.h b/include/coal/internal/BV_splitter.h new file mode 100644 index 000000000..9bfdf459e --- /dev/null +++ b/include/coal/internal/BV_splitter.h @@ -0,0 +1,283 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_BV_SPLITTER_H +#define COAL_BV_SPLITTER_H + +#include "coal/BVH/BVH_internal.h" +#include "coal/BV/kIOS.h" +#include "coal/BV/OBBRSS.h" +#include +#include + +namespace coal { + +/// @brief Three types of split algorithms are provided in FCL as default +enum SplitMethodType { + SPLIT_METHOD_MEAN, + SPLIT_METHOD_MEDIAN, + SPLIT_METHOD_BV_CENTER +}; + +/// @brief A class describing the split rule that splits each BV node +template +class BVSplitter { + public: + BVSplitter(SplitMethodType method) + : split_vector(0, 0, 0), split_method(method) {} + + /// @brief Default deconstructor + virtual ~BVSplitter() {} + + /// @brief Set the geometry data needed by the split rule + void set(Vec3s* vertices_, Triangle* tri_indices_, BVHModelType type_) { + vertices = vertices_; + tri_indices = tri_indices_; + type = type_; + } + + /// @brief Compute the split rule according to a subset of geometry and the + /// corresponding BV node + void computeRule(const BV& bv, unsigned int* primitive_indices, + unsigned int num_primitives) { + switch (split_method) { + case SPLIT_METHOD_MEAN: + computeRule_mean(bv, primitive_indices, num_primitives); + break; + case SPLIT_METHOD_MEDIAN: + computeRule_median(bv, primitive_indices, num_primitives); + break; + case SPLIT_METHOD_BV_CENTER: + computeRule_bvcenter(bv, primitive_indices, num_primitives); + break; + default: + std::cerr << "Split method not supported" << std::endl; + } + } + + /// @brief Apply the split rule on a given point + bool apply(const Vec3s& q) const { return q[split_axis] > split_value; } + + /// @brief Clear the geometry data set before + void clear() { + vertices = NULL; + tri_indices = NULL; + type = BVH_MODEL_UNKNOWN; + } + + protected: + /// @brief The axis based on which the split decision is made. For most BV, + /// the axis is aligned with one of the world coordinate, so only split_axis + /// is needed. For oriented node, we can use a vector to make a better split + /// decision. + int split_axis; + Vec3s split_vector; + + /// @brief The split threshold, different primitives are splitted according + /// whether their projection on the split_axis is larger or smaller than the + /// threshold + CoalScalar split_value; + + /// @brief The mesh vertices or points handled by the splitter + Vec3s* vertices; + + /// @brief The triangles handled by the splitter + Triangle* tri_indices; + + /// @brief Whether the geometry is mesh or point cloud + BVHModelType type; + + /// @brief The split algorithm used + SplitMethodType split_method; + + /// @brief Split algorithm 1: Split the node from center + void computeRule_bvcenter(const BV& bv, unsigned int*, unsigned int) { + Vec3s center = bv.center(); + int axis = 2; + + if (bv.width() >= bv.height() && bv.width() >= bv.depth()) + axis = 0; + else if (bv.height() >= bv.width() && bv.height() >= bv.depth()) + axis = 1; + + split_axis = axis; + split_value = center[axis]; + } + + /// @brief Split algorithm 2: Split the node according to the mean of the data + /// contained + void computeRule_mean(const BV& bv, unsigned int* primitive_indices, + unsigned int num_primitives) { + int axis = 2; + + if (bv.width() >= bv.height() && bv.width() >= bv.depth()) + axis = 0; + else if (bv.height() >= bv.width() && bv.height() >= bv.depth()) + axis = 1; + + split_axis = axis; + CoalScalar sum = 0; + + if (type == BVH_MODEL_TRIANGLES) { + for (unsigned int i = 0; i < num_primitives; ++i) { + const Triangle& t = tri_indices[primitive_indices[i]]; + sum += (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + + vertices[t[2]][split_axis]); + } + + sum /= 3; + } else if (type == BVH_MODEL_POINTCLOUD) { + for (unsigned int i = 0; i < num_primitives; ++i) { + sum += vertices[primitive_indices[i]][split_axis]; + } + } + + split_value = sum / num_primitives; + } + + /// @brief Split algorithm 3: Split the node according to the median of the + /// data contained + void computeRule_median(const BV& bv, unsigned int* primitive_indices, + unsigned int num_primitives) { + int axis = 2; + + if (bv.width() >= bv.height() && bv.width() >= bv.depth()) + axis = 0; + else if (bv.height() >= bv.width() && bv.height() >= bv.depth()) + axis = 1; + + split_axis = axis; + std::vector proj((size_t)num_primitives); + + if (type == BVH_MODEL_TRIANGLES) { + for (unsigned int i = 0; i < num_primitives; ++i) { + const Triangle& t = tri_indices[primitive_indices[i]]; + proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + + vertices[t[2]][split_axis]) / + 3; + } + } else if (type == BVH_MODEL_POINTCLOUD) { + for (unsigned int i = 0; i < num_primitives; ++i) + proj[i] = vertices[primitive_indices[i]][split_axis]; + } + + std::sort(proj.begin(), proj.end()); + + if (num_primitives % 2 == 1) { + split_value = proj[(num_primitives - 1) / 2]; + } else { + split_value = + (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2; + } + } +}; + +template <> +bool COAL_DLLAPI BVSplitter::apply(const Vec3s& q) const; + +template <> +bool COAL_DLLAPI BVSplitter::apply(const Vec3s& q) const; + +template <> +bool COAL_DLLAPI BVSplitter::apply(const Vec3s& q) const; + +template <> +bool COAL_DLLAPI BVSplitter::apply(const Vec3s& q) const; + +template <> +void COAL_DLLAPI BVSplitter::computeRule_bvcenter( + const OBB& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void COAL_DLLAPI BVSplitter::computeRule_mean( + const OBB& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void COAL_DLLAPI BVSplitter::computeRule_median( + const OBB& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void COAL_DLLAPI BVSplitter::computeRule_bvcenter( + const RSS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void COAL_DLLAPI BVSplitter::computeRule_mean( + const RSS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void COAL_DLLAPI BVSplitter::computeRule_median( + const RSS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void COAL_DLLAPI BVSplitter::computeRule_bvcenter( + const kIOS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void COAL_DLLAPI BVSplitter::computeRule_mean( + const kIOS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void COAL_DLLAPI BVSplitter::computeRule_median( + const kIOS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void COAL_DLLAPI BVSplitter::computeRule_bvcenter( + const OBBRSS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void COAL_DLLAPI BVSplitter::computeRule_mean( + const OBBRSS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void COAL_DLLAPI BVSplitter::computeRule_median( + const OBBRSS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +} // namespace coal + +#endif diff --git a/include/coal/internal/intersect.h b/include/coal/internal/intersect.h new file mode 100644 index 000000000..350101cff --- /dev/null +++ b/include/coal/internal/intersect.h @@ -0,0 +1,187 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_INTERSECT_H +#define COAL_INTERSECT_H + +/// @cond INTERNAL + +#include "coal/math/transform.h" + +namespace coal { + +/// @brief CCD intersect kernel among primitives +class COAL_DLLAPI Intersect { + public: + static bool buildTrianglePlane(const Vec3s& v1, const Vec3s& v2, + const Vec3s& v3, Vec3s* n, CoalScalar* t); +}; // class Intersect + +/// @brief Project functions +class COAL_DLLAPI Project { + public: + struct COAL_DLLAPI ProjectResult { + /// @brief Parameterization of the projected point (based on the simplex to + /// be projected, use 2 or 3 or 4 of the array) + CoalScalar parameterization[4]; + + /// @brief square distance from the query point to the projected simplex + CoalScalar sqr_distance; + + /// @brief the code of the projection type + unsigned int encode; + + ProjectResult() : sqr_distance(-1), encode(0) {} + }; + + /// @brief Project point p onto line a-b + static ProjectResult projectLine(const Vec3s& a, const Vec3s& b, + const Vec3s& p); + + /// @brief Project point p onto triangle a-b-c + static ProjectResult projectTriangle(const Vec3s& a, const Vec3s& b, + const Vec3s& c, const Vec3s& p); + + /// @brief Project point p onto tetrahedra a-b-c-d + static ProjectResult projectTetrahedra(const Vec3s& a, const Vec3s& b, + const Vec3s& c, const Vec3s& d, + const Vec3s& p); + + /// @brief Project origin (0) onto line a-b + static ProjectResult projectLineOrigin(const Vec3s& a, const Vec3s& b); + + /// @brief Project origin (0) onto triangle a-b-c + static ProjectResult projectTriangleOrigin(const Vec3s& a, const Vec3s& b, + const Vec3s& c); + + /// @brief Project origin (0) onto tetrahedran a-b-c-d + static ProjectResult projectTetrahedraOrigin(const Vec3s& a, const Vec3s& b, + const Vec3s& c, const Vec3s& d); +}; + +/// @brief Triangle distance functions +class COAL_DLLAPI TriangleDistance { + public: + /// @brief Returns closest points between an segment pair. + /// The first segment is P + t * A + /// The second segment is Q + t * B + /// X, Y are the closest points on the two segments + /// VEC is the vector between X and Y + static void segPoints(const Vec3s& P, const Vec3s& A, const Vec3s& Q, + const Vec3s& B, Vec3s& VEC, Vec3s& X, Vec3s& Y); + + /// Compute squared distance between triangles + /// @param S and T are two triangles + /// @retval P, Q closest points if triangles do not intersect. + /// @return squared distance if triangles do not intersect, 0 otherwise. + /// If the triangles are disjoint, P and Q give the closet points of + /// S and T respectively. However, + /// if the triangles overlap, P and Q are basically a random pair of points + /// from the triangles, not coincident points on the intersection of the + /// triangles, as might be expected. + static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3], Vec3s& P, + Vec3s& Q); + + static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2, + const Vec3s& S3, const Vec3s& T1, + const Vec3s& T2, const Vec3s& T3, Vec3s& P, + Vec3s& Q); + + /// Compute squared distance between triangles + /// @param S and T are two triangles + /// @param R, Tl, rotation and translation applied to T, + /// @retval P, Q closest points if triangles do not intersect. + /// @return squared distance if triangles do not intersect, 0 otherwise. + /// If the triangles are disjoint, P and Q give the closet points of + /// S and T respectively. However, + /// if the triangles overlap, P and Q are basically a random pair of points + /// from the triangles, not coincident points on the intersection of the + /// triangles, as might be expected. + static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3], + const Matrix3s& R, const Vec3s& Tl, Vec3s& P, + Vec3s& Q); + + /// Compute squared distance between triangles + /// @param S and T are two triangles + /// @param tf, rotation and translation applied to T, + /// @retval P, Q closest points if triangles do not intersect. + /// @return squared distance if triangles do not intersect, 0 otherwise. + /// If the triangles are disjoint, P and Q give the closet points of + /// S and T respectively. However, + /// if the triangles overlap, P and Q are basically a random pair of points + /// from the triangles, not coincident points on the intersection of the + /// triangles, as might be expected. + static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3], + const Transform3s& tf, Vec3s& P, Vec3s& Q); + + /// Compute squared distance between triangles + /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices + /// @param R, Tl, rotation and translation applied to T1, T2, T3, + /// @retval P, Q closest points if triangles do not intersect. + /// @return squared distance if triangles do not intersect, 0 otherwise. + /// If the triangles are disjoint, P and Q give the closet points of + /// S and T respectively. However, + /// if the triangles overlap, P and Q are basically a random pair of points + /// from the triangles, not coincident points on the intersection of the + /// triangles, as might be expected. + static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2, + const Vec3s& S3, const Vec3s& T1, + const Vec3s& T2, const Vec3s& T3, + const Matrix3s& R, const Vec3s& Tl, Vec3s& P, + Vec3s& Q); + + /// Compute squared distance between triangles + /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices + /// @param tf, rotation and translation applied to T1, T2, T3, + /// @retval P, Q closest points if triangles do not intersect. + /// @return squared distance if triangles do not intersect, 0 otherwise. + /// If the triangles are disjoint, P and Q give the closet points of + /// S and T respectively. However, + /// if the triangles overlap, P and Q are basically a random pair of points + /// from the triangles, not coincident points on the intersection of the + /// triangles, as might be expected. + static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2, + const Vec3s& S3, const Vec3s& T1, + const Vec3s& T2, const Vec3s& T3, + const Transform3s& tf, Vec3s& P, Vec3s& Q); +}; + +} // namespace coal + +/// @endcond + +#endif diff --git a/include/coal/internal/shape_shape_contact_patch_func.h b/include/coal/internal/shape_shape_contact_patch_func.h new file mode 100644 index 000000000..3eb28b539 --- /dev/null +++ b/include/coal/internal/shape_shape_contact_patch_func.h @@ -0,0 +1,264 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Louis Montaut */ + +#ifndef COAL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H +#define COAL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H + +#include "coal/collision_data.h" +#include "coal/collision_utility.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/contact_patch/contact_patch_solver.h" +#include "coal/shape/geometric_shapes_traits.h" + +namespace coal { + +/// @brief Shape-shape contact patch computation. +/// Assumes that `csolver` and the `ContactPatchResult` have already been set up +/// by the `ContactPatchRequest`. +template +struct ComputeShapeShapeContactPatch { + static void run(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const CollisionResult& collision_result, + const ContactPatchSolver* csolver, + const ContactPatchRequest& request, + ContactPatchResult& result) { + // Note: see specializations for Plane and Halfspace below. + if (!collision_result.isCollision()) { + return; + } + COAL_ASSERT( + result.check(request), + "The contact patch result and request are incompatible (issue of " + "contact patch size or maximum number of contact patches). Make sure " + "result is initialized with request.", + std::logic_error); + + const ShapeType1& s1 = static_cast(*o1); + const ShapeType2& s2 = static_cast(*o2); + for (size_t i = 0; i < collision_result.numContacts(); ++i) { + if (i >= request.max_num_patch) { + break; + } + csolver->setSupportGuess(collision_result.cached_support_func_guess); + const Contact& contact = collision_result.getContact(i); + ContactPatch& contact_patch = result.getUnusedContactPatch(); + csolver->computePatch(s1, tf1, s2, tf2, contact, contact_patch); + } + } +}; + +/// @brief Computes the contact patch between a Plane/Halfspace and another +/// shape. +/// @tparam InvertShapes set to true if the first shape of the collision pair +/// is s2 and not s1 (if you had to invert (s1, tf1) and (s2, tf2) when calling +/// this function). +template +void computePatchPlaneOrHalfspace(const OtherShapeType& s1, + const Transform3s& tf1, + const PlaneOrHalfspace& s2, + const Transform3s& tf2, + const ContactPatchSolver* csolver, + const Contact& contact, + ContactPatch& contact_patch) { + COAL_UNUSED_VARIABLE(s2); + COAL_UNUSED_VARIABLE(tf2); + constructContactPatchFrameFromContact(contact, contact_patch); + if ((bool)(shape_traits::IsStrictlyConvex)) { + // Only one point of contact; it has already been computed. + contact_patch.addPoint(contact.pos); + return; + } + + // We only need to compute the support set in the direction of the normal. + // We need to temporarily express the patch in the local frame of shape1. + SupportSet& support_set = csolver->support_set_shape1; + support_set.tf.rotation().noalias() = + tf1.rotation().transpose() * contact_patch.tf.rotation(); + support_set.tf.translation().noalias() = + tf1.rotation().transpose() * + (contact_patch.tf.translation() - tf1.translation()); + + // Note: for now, taking into account swept-sphere radius does not change + // anything to the support set computations. However it will be used in the + // future if we want to store the offsets to the support plane for each point + // in a support set. + using SupportOptions = details::SupportOptions; + if (InvertShapes) { + support_set.direction = ContactPatch::PatchDirection::INVERTED; + details::getShapeSupportSet( + &s1, support_set, csolver->support_guess[1], csolver->supports_data[1], + csolver->num_samples_curved_shapes, csolver->patch_tolerance); + } else { + support_set.direction = ContactPatch::PatchDirection::DEFAULT; + details::getShapeSupportSet( + &s1, support_set, csolver->support_guess[0], csolver->supports_data[0], + csolver->num_samples_curved_shapes, csolver->patch_tolerance); + } + csolver->getResult(contact, &(support_set.points()), contact_patch); +} + +#define PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(PlaneOrHspace) \ + template \ + struct ComputeShapeShapeContactPatch { \ + static void run(const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ + const CollisionResult& collision_result, \ + const ContactPatchSolver* csolver, \ + const ContactPatchRequest& request, \ + ContactPatchResult& result) { \ + if (!collision_result.isCollision()) { \ + return; \ + } \ + COAL_ASSERT( \ + result.check(request), \ + "The contact patch result and request are incompatible (issue of " \ + "contact patch size or maximum number of contact patches). Make " \ + "sure " \ + "result is initialized with request.", \ + std::logic_error); \ + \ + const OtherShapeType& s1 = static_cast(*o1); \ + const PlaneOrHspace& s2 = static_cast(*o2); \ + for (size_t i = 0; i < collision_result.numContacts(); ++i) { \ + if (i >= request.max_num_patch) { \ + break; \ + } \ + csolver->setSupportGuess(collision_result.cached_support_func_guess); \ + const Contact& contact = collision_result.getContact(i); \ + ContactPatch& contact_patch = result.getUnusedContactPatch(); \ + computePatchPlaneOrHalfspace( \ + s1, tf1, s2, tf2, csolver, contact, contact_patch); \ + } \ + } \ + }; \ + \ + template \ + struct ComputeShapeShapeContactPatch { \ + static void run(const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ + const CollisionResult& collision_result, \ + const ContactPatchSolver* csolver, \ + const ContactPatchRequest& request, \ + ContactPatchResult& result) { \ + if (!collision_result.isCollision()) { \ + return; \ + } \ + COAL_ASSERT( \ + result.check(request), \ + "The contact patch result and request are incompatible (issue of " \ + "contact patch size or maximum number of contact patches). Make " \ + "sure " \ + "result is initialized with request.", \ + std::logic_error); \ + \ + const PlaneOrHspace& s1 = static_cast(*o1); \ + const OtherShapeType& s2 = static_cast(*o2); \ + for (size_t i = 0; i < collision_result.numContacts(); ++i) { \ + if (i >= request.max_num_patch) { \ + break; \ + } \ + csolver->setSupportGuess(collision_result.cached_support_func_guess); \ + const Contact& contact = collision_result.getContact(i); \ + ContactPatch& contact_patch = result.getUnusedContactPatch(); \ + computePatchPlaneOrHalfspace( \ + s2, tf2, s1, tf1, csolver, contact, contact_patch); \ + } \ + } \ + }; + +PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Plane) +PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Halfspace) + +#define PLANE_HSPACE_CONTACT_PATCH(PlaneOrHspace1, PlaneOrHspace2) \ + template <> \ + struct ComputeShapeShapeContactPatch { \ + static void run(const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ + const CollisionResult& collision_result, \ + const ContactPatchSolver* csolver, \ + const ContactPatchRequest& request, \ + ContactPatchResult& result) { \ + COAL_UNUSED_VARIABLE(o1); \ + COAL_UNUSED_VARIABLE(tf1); \ + COAL_UNUSED_VARIABLE(o2); \ + COAL_UNUSED_VARIABLE(tf2); \ + COAL_UNUSED_VARIABLE(csolver); \ + if (!collision_result.isCollision()) { \ + return; \ + } \ + COAL_ASSERT( \ + result.check(request), \ + "The contact patch result and request are incompatible (issue of " \ + "contact patch size or maximum number of contact patches). Make " \ + "sure " \ + "result is initialized with request.", \ + std::logic_error); \ + \ + for (size_t i = 0; i < collision_result.numContacts(); ++i) { \ + if (i >= request.max_num_patch) { \ + break; \ + } \ + const Contact& contact = collision_result.getContact(i); \ + ContactPatch& contact_patch = result.getUnusedContactPatch(); \ + constructContactPatchFrameFromContact(contact, contact_patch); \ + contact_patch.addPoint(contact.pos); \ + } \ + } \ + }; + +PLANE_HSPACE_CONTACT_PATCH(Plane, Plane) +PLANE_HSPACE_CONTACT_PATCH(Plane, Halfspace) +PLANE_HSPACE_CONTACT_PATCH(Halfspace, Plane) +PLANE_HSPACE_CONTACT_PATCH(Halfspace, Halfspace) + +#undef PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH +#undef PLANE_HSPACE_CONTACT_PATCH + +template +void ShapeShapeContactPatch(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const CollisionResult& collision_result, + const ContactPatchSolver* csolver, + const ContactPatchRequest& request, + ContactPatchResult& result) { + return ComputeShapeShapeContactPatch::run( + o1, tf1, o2, tf2, collision_result, csolver, request, result); +} + +} // namespace coal + +#endif diff --git a/include/coal/internal/shape_shape_func.h b/include/coal/internal/shape_shape_func.h new file mode 100644 index 000000000..eb18b3ae9 --- /dev/null +++ b/include/coal/internal/shape_shape_func.h @@ -0,0 +1,318 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, CNRS-LAAS + * Copyright (c) 2024, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Florent Lamiraux */ + +#ifndef COAL_INTERNAL_SHAPE_SHAPE_FUNC_H +#define COAL_INTERNAL_SHAPE_SHAPE_FUNC_H + +/// @cond INTERNAL + +#include "coal/collision_data.h" +#include "coal/collision_utility.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shapes_traits.h" + +namespace coal { + +template +struct ShapeShapeDistancer { + static CoalScalar run(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) { + if (request.isSatisfied(result)) return result.min_distance; + + // Witness points on shape1 and shape2, normal pointing from shape1 to + // shape2. + Vec3s p1, p2, normal; + const CoalScalar distance = + ShapeShapeDistancer::run( + o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, p1, p2, + normal); + + result.update(distance, o1, o2, DistanceResult::NONE, DistanceResult::NONE, + p1, p2, normal); + + return distance; + } + + static CoalScalar run(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const GJKSolver* nsolver, + const bool compute_signed_distance, Vec3s& p1, + Vec3s& p2, Vec3s& normal) { + const ShapeType1* obj1 = static_cast(o1); + const ShapeType2* obj2 = static_cast(o2); + return nsolver->shapeDistance(*obj1, tf1, *obj2, tf2, + compute_signed_distance, p1, p2, normal); + } +}; + +/// @brief Shape-shape distance computation. +/// Assumes that `nsolver` has already been set up by a `DistanceRequest` or +/// a `CollisionRequest`. +/// +/// @note This function is typically used for collision pairs containing two +/// primitive shapes. +/// @note This function might be specialized for some pairs of shapes. +template +CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) { + return ShapeShapeDistancer::run( + o1, tf1, o2, tf2, nsolver, request, result); +} + +namespace internal { +/// @brief Shape-shape distance computation. +/// Assumes that `nsolver` has already been set up by a `DistanceRequest` or +/// a `CollisionRequest`. +/// +/// @note This function is typically used for collision pairs complex structures +/// like BVHs, HeightFields or Octrees. These structures contain sets of +/// primitive shapes. +/// This function is meant to be called on the pairs of primitive shapes of +/// these structures. +/// @note This function might be specialized for some pairs of shapes. +template +CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, const GJKSolver* nsolver, + const bool compute_signed_distance, Vec3s& p1, + Vec3s& p2, Vec3s& normal) { + return ::coal::ShapeShapeDistancer::run( + o1, tf1, o2, tf2, nsolver, compute_signed_distance, p1, p2, normal); +} +} // namespace internal + +/// @brief Shape-shape collision detection. +/// Assumes that `nsolver` has already been set up by a `DistanceRequest` or +/// a `CollisionRequest`. +/// +/// @note This function is typically used for collision pairs containing two +/// primitive shapes. +/// Complex structures like BVHs, HeightFields or Octrees contain sets of +/// primitive shapes should use the `ShapeShapeDistance` function to do their +/// internal collision detection checks. +template +struct ShapeShapeCollider { + static std::size_t run(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { + if (request.isSatisfied(result)) return result.numContacts(); + + const bool compute_penetration = + request.enable_contact || (request.security_margin < 0); + Vec3s p1, p2, normal; + CoalScalar distance = internal::ShapeShapeDistance( + o1, tf1, o2, tf2, nsolver, compute_penetration, p1, p2, normal); + + size_t num_contacts = 0; + const CoalScalar distToCollision = distance - request.security_margin; + + internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision, + p1, p2, normal); + if (distToCollision <= request.collision_distance_threshold && + result.numContacts() < request.num_max_contacts) { + if (result.numContacts() < request.num_max_contacts) { + Contact contact(o1, o2, Contact::NONE, Contact::NONE, p1, p2, normal, + distance); + result.addContact(contact); + } + num_contacts = result.numContacts(); + } + + return num_contacts; + } +}; + +template +std::size_t ShapeShapeCollide(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { + return ShapeShapeCollider::run( + o1, tf1, o2, tf2, nsolver, request, result); +} + +// clang-format off +// ============================================================================================================== +// ============================================================================================================== +// ============================================================================================================== +// Shape distance algorithms based on: +// - built-in function: 0 +// - GJK: 1 +// +// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ +// | | box | sphere | capsule | cone | cylinder | plane | half-space | triangle | ellipsoid | convex | +// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ +// | box | 1 | 0 | 1 | 1 | 1 | 0 | 0 | 1 | 1 | 1 | +// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ +// | sphere |/////| 0 | 0 | 1 | 0 | 0 | 0 | 0 | 1 | 1 | +// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ +// | capsule |/////|////////| 0 | 1 | 1 | 0 | 0 | 1 | 1 | 1 | +// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ +// | cone |/////|////////|/////////| 1 | 1 | 0 | 0 | 1 | 1 | 1 | +// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ +// | cylinder |/////|////////|/////////|//////| 1 | 0 | 0 | 1 | 1 | 1 | +// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ +// | plane |/////|////////|/////////|//////|//////////| ? | ? | 0 | 0 | 0 | +// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ +// | half-space |/////|////////|/////////|//////|//////////|///////| ? | 0 | 0 | 0 | +// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ +// | triangle |/////|////////|/////////|//////|//////////|///////|////////////| 0 | 1 | 1 | +// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ +// | ellipsoid |/////|////////|/////////|//////|//////////|///////|////////////|//////////| 1 | 1 | +// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ +// | convex |/////|////////|/////////|//////|//////////|///////|////////////|//////////|///////////| 1 | +// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ +// +// Number of pairs: 55 +// - Specialized: 26 +// - GJK: 29 +// clang-format on + +#define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2) \ + template <> \ + COAL_DLLAPI CoalScalar internal::ShapeShapeDistance( \ + const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ + const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \ + Vec3s& p2, Vec3s& normal); \ + template <> \ + COAL_DLLAPI CoalScalar internal::ShapeShapeDistance( \ + const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ + const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \ + Vec3s& p2, Vec3s& normal); \ + template <> \ + inline COAL_DLLAPI CoalScalar ShapeShapeDistance( \ + const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ + const GJKSolver* nsolver, const DistanceRequest& request, \ + DistanceResult& result) { \ + result.o1 = o1; \ + result.o2 = o2; \ + result.b1 = DistanceResult::NONE; \ + result.b2 = DistanceResult::NONE; \ + result.min_distance = internal::ShapeShapeDistance( \ + o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \ + result.nearest_points[0], result.nearest_points[1], result.normal); \ + return result.min_distance; \ + } \ + template <> \ + inline COAL_DLLAPI CoalScalar ShapeShapeDistance( \ + const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ + const GJKSolver* nsolver, const DistanceRequest& request, \ + DistanceResult& result) { \ + result.o1 = o1; \ + result.o2 = o2; \ + result.b1 = DistanceResult::NONE; \ + result.b2 = DistanceResult::NONE; \ + result.min_distance = internal::ShapeShapeDistance( \ + o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \ + result.nearest_points[0], result.nearest_points[1], result.normal); \ + return result.min_distance; \ + } + +#define SHAPE_SELF_DISTANCE_SPECIALIZATION(T) \ + template <> \ + COAL_DLLAPI CoalScalar internal::ShapeShapeDistance( \ + const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ + const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \ + Vec3s& p2, Vec3s& normal); \ + template <> \ + inline COAL_DLLAPI CoalScalar ShapeShapeDistance( \ + const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ + const GJKSolver* nsolver, const DistanceRequest& request, \ + DistanceResult& result) { \ + result.o1 = o1; \ + result.o2 = o2; \ + result.b1 = DistanceResult::NONE; \ + result.b2 = DistanceResult::NONE; \ + result.min_distance = internal::ShapeShapeDistance( \ + o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \ + result.nearest_points[0], result.nearest_points[1], result.normal); \ + return result.min_distance; \ + } + +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Halfspace) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Plane) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Sphere) +SHAPE_SELF_DISTANCE_SPECIALIZATION(Capsule) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Halfspace) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Plane) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Halfspace) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Plane) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Halfspace) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Plane) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Halfspace) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Plane) +SHAPE_SELF_DISTANCE_SPECIALIZATION(Sphere) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Halfspace) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Plane) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Halfspace) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Plane) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Plane) +SHAPE_SELF_DISTANCE_SPECIALIZATION(TriangleP) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Sphere) +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Plane, Halfspace) +SHAPE_SELF_DISTANCE_SPECIALIZATION(Plane) +SHAPE_SELF_DISTANCE_SPECIALIZATION(Halfspace) + +#undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION +#undef SHAPE_SELF_DISTANCE_SPECIALIZATION + +} // namespace coal + +/// @endcond + +#endif diff --git a/include/coal/internal/tools.h b/include/coal/internal/tools.h new file mode 100644 index 000000000..ab26633d1 --- /dev/null +++ b/include/coal/internal/tools.h @@ -0,0 +1,213 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Joseph Mirabel */ + +#ifndef COAL_INTERNAL_TOOLS_H +#define COAL_INTERNAL_TOOLS_H + +#include "coal/fwd.hh" + +#include +#include +#include + +#include "coal/data_types.h" + +namespace coal { + +template +static inline typename Derived::Scalar triple( + const Eigen::MatrixBase& x, const Eigen::MatrixBase& y, + const Eigen::MatrixBase& z) { + return x.derived().dot(y.derived().cross(z.derived())); +} + +template +void generateCoordinateSystem(const Eigen::MatrixBase& _w, + const Eigen::MatrixBase& _u, + const Eigen::MatrixBase& _v) { + typedef typename Derived1::Scalar T; + + Eigen::MatrixBase& w = const_cast&>(_w); + Eigen::MatrixBase& u = const_cast&>(_u); + Eigen::MatrixBase& v = const_cast&>(_v); + + T inv_length; + if (std::abs(w[0]) >= std::abs(w[1])) { + inv_length = (T)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]); + u[0] = -w[2] * inv_length; + u[1] = (T)0; + u[2] = w[0] * inv_length; + v[0] = w[1] * u[2]; + v[1] = w[2] * u[0] - w[0] * u[2]; + v[2] = -w[1] * u[0]; + } else { + inv_length = (T)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]); + u[0] = (T)0; + u[1] = w[2] * inv_length; + u[2] = -w[1] * inv_length; + v[0] = w[1] * u[2] - w[2] * u[1]; + v[1] = -w[0] * u[2]; + v[2] = w[0] * u[1]; + } +} + +/* ----- Start Matrices ------ */ +template +void relativeTransform(const Eigen::MatrixBase& R1, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& R2, + const Eigen::MatrixBase& t2, + const Eigen::MatrixBase& R, + const Eigen::MatrixBase& t) { + const_cast&>(R) = R1.transpose() * R2; + const_cast&>(t) = R1.transpose() * (t2 - t1); +} + +/// @brief compute the eigen vector and eigen vector of a matrix. dout is the +/// eigen values, vout is the eigen vectors +template +void eigen(const Eigen::MatrixBase& m, + typename Derived::Scalar dout[3], Vector* vout) { + typedef typename Derived::Scalar S; + Derived R(m.derived()); + int n = 3; + int j, iq, ip, i; + S tresh, theta, tau, t, sm, s, h, g, c; + + S b[3]; + S z[3]; + S v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + S d[3]; + + for (ip = 0; ip < n; ++ip) { + b[ip] = d[ip] = R(ip, ip); + z[ip] = 0; + } + + for (i = 0; i < 50; ++i) { + sm = 0; + for (ip = 0; ip < n; ++ip) + for (iq = ip + 1; iq < n; ++iq) sm += std::abs(R(ip, iq)); + if (sm == 0.0) { + vout[0] << v[0][0], v[0][1], v[0][2]; + vout[1] << v[1][0], v[1][1], v[1][2]; + vout[2] << v[2][0], v[2][1], v[2][2]; + dout[0] = d[0]; + dout[1] = d[1]; + dout[2] = d[2]; + return; + } + + if (i < 3) + tresh = 0.2 * sm / (n * n); + else + tresh = 0.0; + + for (ip = 0; ip < n; ++ip) { + for (iq = ip + 1; iq < n; ++iq) { + g = 100.0 * std::abs(R(ip, iq)); + if (i > 3 && std::abs(d[ip]) + g == std::abs(d[ip]) && + std::abs(d[iq]) + g == std::abs(d[iq])) + R(ip, iq) = 0.0; + else if (std::abs(R(ip, iq)) > tresh) { + h = d[iq] - d[ip]; + if (std::abs(h) + g == std::abs(h)) + t = (R(ip, iq)) / h; + else { + theta = 0.5 * h / (R(ip, iq)); + t = 1.0 / (std::abs(theta) + std::sqrt(1.0 + theta * theta)); + if (theta < 0.0) t = -t; + } + c = 1.0 / std::sqrt(1 + t * t); + s = t * c; + tau = s / (1.0 + c); + h = t * R(ip, iq); + z[ip] -= h; + z[iq] += h; + d[ip] -= h; + d[iq] += h; + R(ip, iq) = 0.0; + for (j = 0; j < ip; ++j) { + g = R(j, ip); + h = R(j, iq); + R(j, ip) = g - s * (h + g * tau); + R(j, iq) = h + s * (g - h * tau); + } + for (j = ip + 1; j < iq; ++j) { + g = R(ip, j); + h = R(j, iq); + R(ip, j) = g - s * (h + g * tau); + R(j, iq) = h + s * (g - h * tau); + } + for (j = iq + 1; j < n; ++j) { + g = R(ip, j); + h = R(iq, j); + R(ip, j) = g - s * (h + g * tau); + R(iq, j) = h + s * (g - h * tau); + } + for (j = 0; j < n; ++j) { + g = v[j][ip]; + h = v[j][iq]; + v[j][ip] = g - s * (h + g * tau); + v[j][iq] = h + s * (g - h * tau); + } + } + } + } + for (ip = 0; ip < n; ++ip) { + b[ip] += z[ip]; + d[ip] = b[ip]; + z[ip] = 0.0; + } + } + + std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl; + + return; +} + +template +bool isEqual(const Eigen::MatrixBase& lhs, + const Eigen::MatrixBase& rhs, + const CoalScalar tol = std::numeric_limits::epsilon() * + 100) { + return ((lhs - rhs).array().abs() < tol).all(); +} + +} // namespace coal + +#endif diff --git a/include/coal/internal/traversal.h b/include/coal/internal/traversal.h new file mode 100644 index 000000000..32fcac457 --- /dev/null +++ b/include/coal/internal/traversal.h @@ -0,0 +1,73 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, LAAS CNRS + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Joseph Mirabel */ + +#ifndef COAL_TRAVERSAL_DETAILS_TRAVERSAL_H +#define COAL_TRAVERSAL_DETAILS_TRAVERSAL_H + +/// @cond INTERNAL + +namespace coal { + +enum { RelativeTransformationIsIdentity = 1 }; + +namespace details { +template +struct COAL_DLLAPI RelativeTransformation { + RelativeTransformation() : R(Matrix3s::Identity()) {} + + const Matrix3s& _R() const { return R; } + const Vec3s& _T() const { return T; } + + Matrix3s R; + Vec3s T; +}; + +template <> +struct COAL_DLLAPI RelativeTransformation { + static const Matrix3s& _R() { + COAL_THROW_PRETTY("should never reach this point", std::logic_error); + } + static const Vec3s& _T() { + COAL_THROW_PRETTY("should never reach this point", std::logic_error); + } +}; +} // namespace details + +} // namespace coal + +/// @endcond + +#endif diff --git a/include/coal/internal/traversal_node_base.h b/include/coal/internal/traversal_node_base.h new file mode 100644 index 000000000..edd252985 --- /dev/null +++ b/include/coal/internal/traversal_node_base.h @@ -0,0 +1,172 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_TRAVERSAL_NODE_BASE_H +#define COAL_TRAVERSAL_NODE_BASE_H + +/// @cond INTERNAL + +#include "coal/data_types.h" +#include "coal/math/transform.h" +#include "coal/collision_data.h" + +namespace coal { + +/// @brief Node structure encoding the information required for traversal. + +class TraversalNodeBase { + public: + TraversalNodeBase() : enable_statistics(false) {} + + virtual ~TraversalNodeBase() {} + + virtual void preprocess() {} + + virtual void postprocess() {} + + /// @brief Whether b is a leaf node in the first BVH tree + virtual bool isFirstNodeLeaf(unsigned int /*b*/) const { return true; } + + /// @brief Whether b is a leaf node in the second BVH tree + virtual bool isSecondNodeLeaf(unsigned int /*b*/) const { return true; } + + /// @brief Traverse the subtree of the node in the first tree first + virtual bool firstOverSecond(unsigned int /*b1*/, unsigned int /*b2*/) const { + return true; + } + + /// @brief Get the left child of the node b in the first tree + virtual int getFirstLeftChild(unsigned int b) const { return (int)b; } + + /// @brief Get the right child of the node b in the first tree + virtual int getFirstRightChild(unsigned int b) const { return (int)b; } + + /// @brief Get the left child of the node b in the second tree + virtual int getSecondLeftChild(unsigned int b) const { return (int)b; } + + /// @brief Get the right child of the node b in the second tree + virtual int getSecondRightChild(unsigned int b) const { return (int)b; } + + /// @brief Whether store some statistics information during traversal + void enableStatistics(bool enable) { enable_statistics = enable; } + + /// @brief configuation of first object + Transform3s tf1; + + /// @brief configuration of second object + Transform3s tf2; + + /// @brief Whether stores statistics + bool enable_statistics; +}; + +/// @defgroup Traversal_For_Collision +/// regroup class about traversal for distance. +/// @{ + +/// @brief Node structure encoding the information required for collision +/// traversal. +class CollisionTraversalNodeBase : public TraversalNodeBase { + public: + CollisionTraversalNodeBase(const CollisionRequest& request_) + : request(request_), result(NULL) {} + + virtual ~CollisionTraversalNodeBase() {} + + /// BV test between b1 and b2 + /// @param b1, b2 Bounding volumes to test, + /// @retval sqrDistLowerBound square of a lower bound of the minimal + /// distance between bounding volumes. + virtual bool BVDisjoints(unsigned int b1, unsigned int b2, + CoalScalar& sqrDistLowerBound) const = 0; + + /// @brief Leaf test between node b1 and b2, if they are both leafs + virtual void leafCollides(unsigned int /*b1*/, unsigned int /*b2*/, + CoalScalar& /*sqrDistLowerBound*/) const = 0; + + /// @brief Check whether the traversal can stop + bool canStop() const { return this->request.isSatisfied(*(this->result)); } + + /// @brief request setting for collision + const CollisionRequest& request; + + /// @brief collision result kept during the traversal iteration + CollisionResult* result; +}; + +/// @} + +/// @defgroup Traversal_For_Distance +/// regroup class about traversal for distance. +/// @{ + +/// @brief Node structure encoding the information required for distance +/// traversal. +class DistanceTraversalNodeBase : public TraversalNodeBase { + public: + DistanceTraversalNodeBase() : result(NULL) {} + + virtual ~DistanceTraversalNodeBase() {} + + /// @brief BV test between b1 and b2 + /// @return a lower bound of the distance between the two BV. + /// @note except for OBB, this method returns the distance. + virtual CoalScalar BVDistanceLowerBound(unsigned int /*b1*/, + unsigned int /*b2*/) const { + return (std::numeric_limits::max)(); + } + + /// @brief Leaf test between node b1 and b2, if they are both leafs + virtual void leafComputeDistance(unsigned int b1, unsigned int b2) const = 0; + + /// @brief Check whether the traversal can stop + virtual bool canStop(CoalScalar /*c*/) const { return false; } + + /// @brief request setting for distance + DistanceRequest request; + + /// @brief distance result kept during the traversal iteration + DistanceResult* result; +}; + +///@} + +} // namespace coal + +/// @endcond + +#endif diff --git a/include/hpp/fcl/internal/traversal_node_bvh_hfield.h b/include/coal/internal/traversal_node_bvh_hfield.h similarity index 84% rename from include/hpp/fcl/internal/traversal_node_bvh_hfield.h rename to include/coal/internal/traversal_node_bvh_hfield.h index 27a0caed5..fc550eb21 100644 --- a/include/hpp/fcl/internal/traversal_node_bvh_hfield.h +++ b/include/coal/internal/traversal_node_bvh_hfield.h @@ -34,29 +34,28 @@ /** \author Justin Carpentier */ -#ifndef HPP_FCL_TRAVERSAL_NODE_BVH_HFIELD_H -#define HPP_FCL_TRAVERSAL_NODE_BVH_HFIELD_H +#ifndef COAL_TRAVERSAL_NODE_BVH_HFIELD_H +#define COAL_TRAVERSAL_NODE_BVH_HFIELD_H /// @cond INTERNAL -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "coal/collision_data.h" +#include "coal/internal/traversal_node_base.h" +#include "coal/internal/traversal_node_hfield_shape.h" +#include "coal/BV/BV_node.h" +#include "coal/BV/BV.h" +#include "coal/BVH/BVH_model.h" +#include "coal/hfield.h" +#include "coal/internal/intersect.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/internal/traversal.h" #include #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @addtogroup Traversal_For_Collision /// @{ @@ -100,8 +99,8 @@ class MeshHeightFieldCollisionTraversalNode /// @brief Determine the traversal order, is the first BVTT subtree better bool firstOverSecond(unsigned int b1, unsigned int b2) const { - FCL_REAL sz1 = model1->getBV(b1).bv.size(); - FCL_REAL sz2 = model2->getBV(b2).bv.size(); + CoalScalar sz1 = model1->getBV(b1).bv.size(); + CoalScalar sz2 = model2->getBV(b2).bv.size(); bool l1 = model1->getBV(b1).isLeaf(); bool l2 = model2->getBV(b2).isLeaf(); @@ -145,7 +144,7 @@ class MeshHeightFieldCollisionTraversalNode /// @retval sqrDistLowerBound square of a lower bound of the minimal /// distance between bounding volumes. bool BVDisjoints(unsigned int b1, unsigned int b2, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { if (this->enable_statistics) this->num_bv_tests++; if (RTIsIdentity) return !this->model1->getBV(b1).overlap(this->model2->getBV(b2), @@ -174,7 +173,7 @@ class MeshHeightFieldCollisionTraversalNode /// and the object are not colliding, the penetration depth is /// negative. void leafCollides(unsigned int b1, unsigned int b2, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { if (this->enable_statistics) this->num_leaf_tests++; const BVNode& node1 = this->model1->getBV(b1); @@ -183,9 +182,9 @@ class MeshHeightFieldCollisionTraversalNode int primitive_id1 = node1.primitiveId(); const Triangle& tri_id1 = tri_indices1[primitive_id1]; - const Vec3f& P1 = vertices1[tri_id1[0]]; - const Vec3f& P2 = vertices1[tri_id1[1]]; - const Vec3f& P3 = vertices1[tri_id1[2]]; + const Vec3s& P1 = vertices1[tri_id1[0]]; + const Vec3s& P2 = vertices1[tri_id1[1]]; + const Vec3s& P3 = vertices1[tri_id1[2]]; TriangleP tri1(P1, P2, P3); @@ -194,17 +193,17 @@ class MeshHeightFieldCollisionTraversalNode details::buildConvexTriangles(node2, *this->model2, convex2, convex2); GJKSolver solver; - Vec3f p1, + Vec3s p1, p2; // closest points if no collision contact points if collision. - Vec3f normal; - FCL_REAL distance; + Vec3s normal; + CoalScalar distance; solver.shapeDistance(tri1, this->tf1, tri2, this->tf2, distance, p1, p2, normal); - FCL_REAL distToCollision = distance - this->request.security_margin; + CoalScalar distToCollision = distance - this->request.security_margin; sqrDistLowerBound = distance * distance; if (distToCollision <= 0) { // collision - Vec3f p(p1); // contact point - FCL_REAL penetrationDepth(0); + Vec3s p(p1); // contact point + CoalScalar penetrationDepth(0); if (this->result->numContacts() < this->request.num_max_contacts) { // How much (Q1, Q2, Q3) should be moved so that all vertices are // above (P1, P2, P3). @@ -228,9 +227,9 @@ class MeshHeightFieldCollisionTraversalNode /// @brief statistical information mutable int num_bv_tests; mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; + mutable CoalScalar query_time_seconds; - Vec3f* vertices1 Triangle* tri_indices1; + Vec3s* vertices1 Triangle* tri_indices1; details::RelativeTransformation RT; }; @@ -251,19 +250,19 @@ typedef MeshHeightFieldCollisionTraversalNode namespace details { template struct DistanceTraversalBVDistanceLowerBound_impl { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) { + static CoalScalar run(const BVNode& b1, const BVNode& b2) { return b1.distance(b2); } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, - const BVNode& b2) { + static CoalScalar run(const Matrix3s& R, const Vec3s& T, const BVNode& b1, + const BVNode& b2) { return distance(R, T, b1.bv, b2.bv); } }; template <> struct DistanceTraversalBVDistanceLowerBound_impl { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) { - FCL_REAL sqrDistLowerBound; + static CoalScalar run(const BVNode& b1, const BVNode& b2) { + CoalScalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (b1.overlap(b2, request, sqrDistLowerBound)) { @@ -272,9 +271,9 @@ struct DistanceTraversalBVDistanceLowerBound_impl { } return sqrt(sqrDistLowerBound); } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, - const BVNode& b2) { - FCL_REAL sqrDistLowerBound; + static CoalScalar run(const Matrix3s& R, const Vec3s& T, + const BVNode& b1, const BVNode& b2) { + CoalScalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { @@ -287,8 +286,8 @@ struct DistanceTraversalBVDistanceLowerBound_impl { template <> struct DistanceTraversalBVDistanceLowerBound_impl { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) { - FCL_REAL sqrDistLowerBound; + static CoalScalar run(const BVNode& b1, const BVNode& b2) { + CoalScalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (b1.overlap(b2, request, sqrDistLowerBound)) { @@ -297,9 +296,9 @@ struct DistanceTraversalBVDistanceLowerBound_impl { } return sqrt(sqrDistLowerBound); } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, - const BVNode& b2) { - FCL_REAL sqrDistLowerBound; + static CoalScalar run(const Matrix3s& R, const Vec3s& T, + const BVNode& b1, const BVNode& b2) { + CoalScalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { @@ -339,8 +338,8 @@ class BVHDistanceTraversalNode : public DistanceTraversalNodeBase { /// @brief Determine the traversal order, is the first BVTT subtree better bool firstOverSecond(unsigned int b1, unsigned int b2) const { - FCL_REAL sz1 = model1->getBV(b1).bv.size(); - FCL_REAL sz2 = model2->getBV(b2).bv.size(); + CoalScalar sz1 = model1->getBV(b1).bv.size(); + CoalScalar sz2 = model2->getBV(b2).bv.size(); bool l1 = model1->getBV(b1).isLeaf(); bool l2 = model2->getBV(b2).isLeaf(); @@ -377,7 +376,7 @@ class BVHDistanceTraversalNode : public DistanceTraversalNodeBase { /// @brief statistical information mutable int num_bv_tests; mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; + mutable CoalScalar query_time_seconds; }; /// @brief Traversal node for distance computation between two meshes @@ -417,7 +416,7 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { } /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int b2) const { + CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int b2) const { if (enable_statistics) num_bv_tests++; if (RTIsIdentity) return details::DistanceTraversalBVDistanceLowerBound_impl::run( @@ -440,47 +439,47 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { const Triangle& tri_id1 = tri_indices1[primitive_id1]; const Triangle& tri_id2 = tri_indices2[primitive_id2]; - const Vec3f& t11 = vertices1[tri_id1[0]]; - const Vec3f& t12 = vertices1[tri_id1[1]]; - const Vec3f& t13 = vertices1[tri_id1[2]]; + const Vec3s& t11 = vertices1[tri_id1[0]]; + const Vec3s& t12 = vertices1[tri_id1[1]]; + const Vec3s& t13 = vertices1[tri_id1[2]]; - const Vec3f& t21 = vertices2[tri_id2[0]]; - const Vec3f& t22 = vertices2[tri_id2[1]]; - const Vec3f& t23 = vertices2[tri_id2[2]]; + const Vec3s& t21 = vertices2[tri_id2[0]]; + const Vec3s& t22 = vertices2[tri_id2[1]]; + const Vec3s& t23 = vertices2[tri_id2[2]]; // nearest point pair - Vec3f P1, P2, normal; + Vec3s P1, P2, normal; - FCL_REAL d2; + CoalScalar d2; if (RTIsIdentity) d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1, P2); else d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, RT._R(), RT._T(), P1, P2); - FCL_REAL d = sqrt(d2); + CoalScalar d = sqrt(d2); this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2, P1, P2, normal); } /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const { + bool canStop(CoalScalar c) const { if ((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) return true; return false; } - Vec3f* vertices1; - Vec3f* vertices2; + Vec3s* vertices1; + Vec3s* vertices2; Triangle* tri_indices1; Triangle* tri_indices2; /// @brief relative and absolute error, default value is 0.01 for both terms - FCL_REAL rel_err; - FCL_REAL abs_err; + CoalScalar rel_err; + CoalScalar abs_err; details::RelativeTransformation RT; @@ -490,8 +489,8 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { const Triangle& init_tri1 = tri_indices1[init_tri_id1]; const Triangle& init_tri2 = tri_indices2[init_tri_id2]; - Vec3f init_tri1_points[3]; - Vec3f init_tri2_points[3]; + Vec3s init_tri1_points[3]; + Vec3s init_tri2_points[3]; init_tri1_points[0] = vertices1[init_tri1[0]]; init_tri1_points[1] = vertices1[init_tri1[1]]; @@ -501,8 +500,8 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { init_tri2_points[1] = vertices2[init_tri2[1]]; init_tri2_points[2] = vertices2[init_tri2[2]]; - Vec3f p1, p2, normal; - FCL_REAL distance = sqrt(TriangleDistance::sqrTriDistance( + Vec3s p1, p2, normal; + CoalScalar distance = sqrt(TriangleDistance::sqrTriDistance( init_tri1_points[0], init_tri1_points[1], init_tri1_points[2], init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(), RT._T(), p1, p2)); @@ -535,20 +534,18 @@ typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeOBBRSS; namespace details { template -inline const Matrix3f& getBVAxes(const BV& bv) { +inline const Matrix3s& getBVAxes(const BV& bv) { return bv.axes; } template <> -inline const Matrix3f& getBVAxes(const OBBRSS& bv) { +inline const Matrix3s& getBVAxes(const OBBRSS& bv) { return bv.obb.axes; } } // namespace details -} // namespace fcl - -} // namespace hpp +} // namespace coal /// @endcond diff --git a/include/coal/internal/traversal_node_bvh_shape.h b/include/coal/internal/traversal_node_bvh_shape.h new file mode 100644 index 000000000..5b85879a5 --- /dev/null +++ b/include/coal/internal/traversal_node_bvh_shape.h @@ -0,0 +1,485 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_TRAVERSAL_NODE_MESH_SHAPE_H +#define COAL_TRAVERSAL_NODE_MESH_SHAPE_H + +/// @cond INTERNAL + +#include "coal/collision_data.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/internal/traversal_node_base.h" +#include "coal/internal/traversal.h" +#include "coal/BVH/BVH_model.h" +#include "coal/internal/shape_shape_func.h" + +namespace coal { + +/// @addtogroup Traversal_For_Collision +/// @{ + +/// @brief Traversal node for collision between BVH and shape +template +class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase { + public: + BVHShapeCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { + model1 = NULL; + model2 = NULL; + + num_bv_tests = 0; + num_leaf_tests = 0; + query_time_seconds = 0.0; + } + + /// @brief Whether the BV node in the first BVH tree is leaf + bool isFirstNodeLeaf(unsigned int b) const { + return model1->getBV(b).isLeaf(); + } + + /// @brief Obtain the left child of BV node in the first BVH + int getFirstLeftChild(unsigned int b) const { + return model1->getBV(b).leftChild(); + } + + /// @brief Obtain the right child of BV node in the first BVH + int getFirstRightChild(unsigned int b) const { + return model1->getBV(b).rightChild(); + } + + const BVHModel* model1; + const S* model2; + BV model2_bv; + + mutable int num_bv_tests; + mutable int num_leaf_tests; + mutable CoalScalar query_time_seconds; +}; + +/// @brief Traversal node for collision between mesh and shape +template +class MeshShapeCollisionTraversalNode + : public BVHShapeCollisionTraversalNode { + public: + enum { + Options = _Options, + RTIsIdentity = _Options & RelativeTransformationIsIdentity + }; + + MeshShapeCollisionTraversalNode(const CollisionRequest& request) + : BVHShapeCollisionTraversalNode(request) { + vertices = NULL; + tri_indices = NULL; + + nsolver = NULL; + } + + /// test between BV b1 and shape + /// @param b1 BV to test, + /// @retval sqrDistLowerBound square of a lower bound of the minimal + /// distance between bounding volumes. + /// @brief BV culling test in one BVTT node + bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, + CoalScalar& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_bv_tests++; + bool disjoint; + if (RTIsIdentity) + disjoint = !this->model1->getBV(b1).bv.overlap( + this->model2_bv, this->request, sqrDistLowerBound); + else + disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), + this->model1->getBV(b1).bv, this->model2_bv, + this->request, sqrDistLowerBound); + if (disjoint) + internal::updateDistanceLowerBoundFromBV(this->request, *this->result, + sqrDistLowerBound); + assert(!disjoint || sqrDistLowerBound > 0); + return disjoint; + } + + /// @brief Intersection testing between leaves (one triangle and one shape) + void leafCollides(unsigned int b1, unsigned int /*b2*/, + CoalScalar& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_leaf_tests++; + const BVNode& node = this->model1->getBV(b1); + + int primitive_id = node.primitiveId(); + + const Triangle& tri_id = this->tri_indices[primitive_id]; + const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]], + this->vertices[tri_id[2]]); + + // When reaching this point, `this->solver` has already been set up + // by the CollisionRequest `this->request`. + // The only thing we need to (and can) pass to `ShapeShapeDistance` is + // whether or not penetration information is should be computed in case of + // collision. + const bool compute_penetration = + this->request.enable_contact || (this->request.security_margin < 0); + Vec3s c1, c2, normal; + CoalScalar distance; + + if (RTIsIdentity) { + static const Transform3s Id; + distance = internal::ShapeShapeDistance( + &tri, Id, this->model2, this->tf2, this->nsolver, compute_penetration, + c1, c2, normal); + } else { + distance = internal::ShapeShapeDistance( + &tri, this->tf1, this->model2, this->tf2, this->nsolver, + compute_penetration, c1, c2, normal); + } + const CoalScalar distToCollision = distance - this->request.security_margin; + + internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result), + distToCollision, c1, c2, normal); + + if (distToCollision <= this->request.collision_distance_threshold) { + sqrDistLowerBound = 0; + if (this->result->numContacts() < this->request.num_max_contacts) { + this->result->addContact(Contact(this->model1, this->model2, + primitive_id, Contact::NONE, c1, c2, + normal, distance)); + assert(this->result->isCollision()); + } + } else { + sqrDistLowerBound = distToCollision * distToCollision; + } + + assert(this->result->isCollision() || sqrDistLowerBound > 0); + } // leafCollides + + Vec3s* vertices; + Triangle* tri_indices; + + const GJKSolver* nsolver; +}; + +/// @} + +/// @addtogroup Traversal_For_Distance +/// @{ + +/// @brief Traversal node for distance computation between BVH and shape +template +class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase { + public: + BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { + model1 = NULL; + model2 = NULL; + + num_bv_tests = 0; + num_leaf_tests = 0; + query_time_seconds = 0.0; + } + + /// @brief Whether the BV node in the first BVH tree is leaf + bool isFirstNodeLeaf(unsigned int b) const { + return model1->getBV(b).isLeaf(); + } + + /// @brief Obtain the left child of BV node in the first BVH + int getFirstLeftChild(unsigned int b) const { + return model1->getBV(b).leftChild(); + } + + /// @brief Obtain the right child of BV node in the first BVH + int getFirstRightChild(unsigned int b) const { + return model1->getBV(b).rightChild(); + } + + /// @brief BV culling test in one BVTT node + CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { + return model1->getBV(b1).bv.distance(model2_bv); + } + + const BVHModel* model1; + const S* model2; + BV model2_bv; + + mutable int num_bv_tests; + mutable int num_leaf_tests; + mutable CoalScalar query_time_seconds; +}; + +/// @brief Traversal node for distance computation between shape and BVH +template +class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase { + public: + ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase() { + model1 = NULL; + model2 = NULL; + + num_bv_tests = 0; + num_leaf_tests = 0; + query_time_seconds = 0.0; + } + + /// @brief Whether the BV node in the second BVH tree is leaf + bool isSecondNodeLeaf(unsigned int b) const { + return model2->getBV(b).isLeaf(); + } + + /// @brief Obtain the left child of BV node in the second BVH + int getSecondLeftChild(unsigned int b) const { + return model2->getBV(b).leftChild(); + } + + /// @brief Obtain the right child of BV node in the second BVH + int getSecondRightChild(unsigned int b) const { + return model2->getBV(b).rightChild(); + } + + /// @brief BV culling test in one BVTT node + CoalScalar BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const { + return model1_bv.distance(model2->getBV(b2).bv); + } + + const S* model1; + const BVHModel* model2; + BV model1_bv; + + mutable int num_bv_tests; + mutable int num_leaf_tests; + mutable CoalScalar query_time_seconds; +}; + +/// @brief Traversal node for distance between mesh and shape +template +class MeshShapeDistanceTraversalNode + : public BVHShapeDistanceTraversalNode { + public: + MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode() { + vertices = NULL; + tri_indices = NULL; + + rel_err = 0; + abs_err = 0; + + nsolver = NULL; + } + + /// @brief Distance testing between leaves (one triangle and one shape) + void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const { + if (this->enable_statistics) this->num_leaf_tests++; + + const BVNode& node = this->model1->getBV(b1); + + int primitive_id = node.primitiveId(); + + const Triangle& tri_id = tri_indices[primitive_id]; + const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]], + this->vertices[tri_id[2]]); + + Vec3s p1, p2, normal; + const CoalScalar distance = internal::ShapeShapeDistance( + &tri, this->tf1, this->model2, this->tf2, this->nsolver, + this->request.enable_signed_distance, p1, p2, normal); + + this->result->update(distance, this->model1, this->model2, primitive_id, + DistanceResult::NONE, p1, p2, normal); + } + + /// @brief Whether the traversal process can stop early + bool canStop(CoalScalar c) const { + if ((c >= this->result->min_distance - abs_err) && + (c * (1 + rel_err) >= this->result->min_distance)) + return true; + return false; + } + + Vec3s* vertices; + Triangle* tri_indices; + + CoalScalar rel_err; + CoalScalar abs_err; + + const GJKSolver* nsolver; +}; + +/// @cond IGNORE +namespace details { + +template +void meshShapeDistanceOrientedNodeleafComputeDistance( + unsigned int b1, unsigned int /* b2 */, const BVHModel* model1, + const S& model2, Vec3s* vertices, Triangle* tri_indices, + const Transform3s& tf1, const Transform3s& tf2, const GJKSolver* nsolver, + bool enable_statistics, int& num_leaf_tests, const DistanceRequest& request, + DistanceResult& result) { + if (enable_statistics) num_leaf_tests++; + + const BVNode& node = model1->getBV(b1); + int primitive_id = node.primitiveId(); + + const Triangle& tri_id = tri_indices[primitive_id]; + const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]], + vertices[tri_id[2]]); + + Vec3s p1, p2, normal; + const CoalScalar distance = internal::ShapeShapeDistance( + &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2, + normal); + + result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE, + p1, p2, normal); +} + +template +static inline void distancePreprocessOrientedNode( + const BVHModel* model1, Vec3s* vertices, Triangle* tri_indices, + int init_tri_id, const S& model2, const Transform3s& tf1, + const Transform3s& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, DistanceResult& result) { + const Triangle& tri_id = tri_indices[init_tri_id]; + const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]], + vertices[tri_id[2]]); + + Vec3s p1, p2, normal; + const CoalScalar distance = internal::ShapeShapeDistance( + &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2, + normal); + result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE, + p1, p2, normal); +} + +} // namespace details + +/// @endcond + +/// @brief Traversal node for distance between mesh and shape, when mesh BVH is +/// one of the oriented node (RSS, kIOS, OBBRSS) +template +class MeshShapeDistanceTraversalNodeRSS + : public MeshShapeDistanceTraversalNode { + public: + MeshShapeDistanceTraversalNodeRSS() + : MeshShapeDistanceTraversalNode() {} + + void preprocess() { + details::distancePreprocessOrientedNode( + this->model1, this->vertices, this->tri_indices, 0, *(this->model2), + this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); + } + + void postprocess() {} + + CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { + if (this->enable_statistics) this->num_bv_tests++; + return distance(this->tf1.getRotation(), this->tf1.getTranslation(), + this->model2_bv, this->model1->getBV(b1).bv); + } + + void leafComputeDistance(unsigned int b1, unsigned int b2) const { + details::meshShapeDistanceOrientedNodeleafComputeDistance( + b1, b2, this->model1, *(this->model2), this->vertices, + this->tri_indices, this->tf1, this->tf2, this->nsolver, + this->enable_statistics, this->num_leaf_tests, this->request, + *(this->result)); + } +}; + +template +class MeshShapeDistanceTraversalNodekIOS + : public MeshShapeDistanceTraversalNode { + public: + MeshShapeDistanceTraversalNodekIOS() + : MeshShapeDistanceTraversalNode() {} + + void preprocess() { + details::distancePreprocessOrientedNode( + this->model1, this->vertices, this->tri_indices, 0, *(this->model2), + this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); + } + + void postprocess() {} + + CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { + if (this->enable_statistics) this->num_bv_tests++; + return distance(this->tf1.getRotation(), this->tf1.getTranslation(), + this->model2_bv, this->model1->getBV(b1).bv); + } + + void leafComputeDistance(unsigned int b1, unsigned int b2) const { + details::meshShapeDistanceOrientedNodeleafComputeDistance( + b1, b2, this->model1, *(this->model2), this->vertices, + this->tri_indices, this->tf1, this->tf2, this->nsolver, + this->enable_statistics, this->num_leaf_tests, this->request, + *(this->result)); + } +}; + +template +class MeshShapeDistanceTraversalNodeOBBRSS + : public MeshShapeDistanceTraversalNode { + public: + MeshShapeDistanceTraversalNodeOBBRSS() + : MeshShapeDistanceTraversalNode() {} + + void preprocess() { + details::distancePreprocessOrientedNode( + this->model1, this->vertices, this->tri_indices, 0, *(this->model2), + this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); + } + + void postprocess() {} + + CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { + if (this->enable_statistics) this->num_bv_tests++; + return distance(this->tf1.getRotation(), this->tf1.getTranslation(), + this->model2_bv, this->model1->getBV(b1).bv); + } + + void leafComputeDistance(unsigned int b1, unsigned int b2) const { + details::meshShapeDistanceOrientedNodeleafComputeDistance( + b1, b2, this->model1, *(this->model2), this->vertices, + this->tri_indices, this->tf1, this->tf2, this->nsolver, + this->enable_statistics, this->num_leaf_tests, this->request, + *(this->result)); + } +}; + +/// @} + +} // namespace coal + +/// @endcond + +#endif diff --git a/include/coal/internal/traversal_node_bvhs.h b/include/coal/internal/traversal_node_bvhs.h new file mode 100644 index 000000000..a7b2446f4 --- /dev/null +++ b/include/coal/internal/traversal_node_bvhs.h @@ -0,0 +1,554 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_TRAVERSAL_NODE_MESHES_H +#define COAL_TRAVERSAL_NODE_MESHES_H + +/// @cond INTERNAL + +#include "coal/collision_data.h" +#include "coal/internal/traversal_node_base.h" +#include "coal/BV/BV_node.h" +#include "coal/BV/BV.h" +#include "coal/BVH/BVH_model.h" +#include "coal/internal/intersect.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/internal/traversal.h" +#include "coal/internal/shape_shape_func.h" + +#include + +namespace coal { + +/// @addtogroup Traversal_For_Collision +/// @{ + +/// @brief Traversal node for collision between BVH models +template +class BVHCollisionTraversalNode : public CollisionTraversalNodeBase { + public: + BVHCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { + model1 = NULL; + model2 = NULL; + + num_bv_tests = 0; + num_leaf_tests = 0; + query_time_seconds = 0.0; + } + + /// @brief Whether the BV node in the first BVH tree is leaf + bool isFirstNodeLeaf(unsigned int b) const { + assert(model1 != NULL && "model1 is NULL"); + return model1->getBV(b).isLeaf(); + } + + /// @brief Whether the BV node in the second BVH tree is leaf + bool isSecondNodeLeaf(unsigned int b) const { + assert(model2 != NULL && "model2 is NULL"); + return model2->getBV(b).isLeaf(); + } + + /// @brief Determine the traversal order, is the first BVTT subtree better + bool firstOverSecond(unsigned int b1, unsigned int b2) const { + CoalScalar sz1 = model1->getBV(b1).bv.size(); + CoalScalar sz2 = model2->getBV(b2).bv.size(); + + bool l1 = model1->getBV(b1).isLeaf(); + bool l2 = model2->getBV(b2).isLeaf(); + + if (l2 || (!l1 && (sz1 > sz2))) return true; + return false; + } + + /// @brief Obtain the left child of BV node in the first BVH + int getFirstLeftChild(unsigned int b) const { + return model1->getBV(b).leftChild(); + } + + /// @brief Obtain the right child of BV node in the first BVH + int getFirstRightChild(unsigned int b) const { + return model1->getBV(b).rightChild(); + } + + /// @brief Obtain the left child of BV node in the second BVH + int getSecondLeftChild(unsigned int b) const { + return model2->getBV(b).leftChild(); + } + + /// @brief Obtain the right child of BV node in the second BVH + int getSecondRightChild(unsigned int b) const { + return model2->getBV(b).rightChild(); + } + + /// @brief The first BVH model + const BVHModel* model1; + /// @brief The second BVH model + const BVHModel* model2; + + /// @brief statistical information + mutable int num_bv_tests; + mutable int num_leaf_tests; + mutable CoalScalar query_time_seconds; +}; + +/// @brief Traversal node for collision between two meshes +template +class MeshCollisionTraversalNode : public BVHCollisionTraversalNode { + public: + enum { + Options = _Options, + RTIsIdentity = _Options & RelativeTransformationIsIdentity + }; + + MeshCollisionTraversalNode(const CollisionRequest& request) + : BVHCollisionTraversalNode(request) { + vertices1 = NULL; + vertices2 = NULL; + tri_indices1 = NULL; + tri_indices2 = NULL; + } + + /// BV test between b1 and b2 + /// @param b1, b2 Bounding volumes to test, + /// @retval sqrDistLowerBound square of a lower bound of the minimal + /// distance between bounding volumes. + bool BVDisjoints(unsigned int b1, unsigned int b2, + CoalScalar& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_bv_tests++; + bool disjoint; + if (RTIsIdentity) + disjoint = !this->model1->getBV(b1).overlap( + this->model2->getBV(b2), this->request, sqrDistLowerBound); + else { + disjoint = !overlap(RT._R(), RT._T(), this->model2->getBV(b2).bv, + this->model1->getBV(b1).bv, this->request, + sqrDistLowerBound); + } + if (disjoint) + internal::updateDistanceLowerBoundFromBV(this->request, *this->result, + sqrDistLowerBound); + return disjoint; + } + + /// Intersection testing between leaves (two triangles) + /// + /// @param b1, b2 id of primitive in bounding volume hierarchy + /// @retval sqrDistLowerBound squared lower bound of distance between + /// primitives if they are not in collision. + /// + /// This method supports a security margin. If the distance between + /// the primitives is less than the security margin, the objects are + /// considered as in collision. in this case a contact point is + /// returned in the CollisionResult. + /// + /// @note If the distance between objects is less than the security margin, + /// and the object are not colliding, the penetration depth is + /// negative. + void leafCollides(unsigned int b1, unsigned int b2, + CoalScalar& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_leaf_tests++; + + const BVNode& node1 = this->model1->getBV(b1); + const BVNode& node2 = this->model2->getBV(b2); + + int primitive_id1 = node1.primitiveId(); + int primitive_id2 = node2.primitiveId(); + + const Triangle& tri_id1 = tri_indices1[primitive_id1]; + const Triangle& tri_id2 = tri_indices2[primitive_id2]; + + const Vec3s& P1 = vertices1[tri_id1[0]]; + const Vec3s& P2 = vertices1[tri_id1[1]]; + const Vec3s& P3 = vertices1[tri_id1[2]]; + const Vec3s& Q1 = vertices2[tri_id2[0]]; + const Vec3s& Q2 = vertices2[tri_id2[1]]; + const Vec3s& Q3 = vertices2[tri_id2[2]]; + + TriangleP tri1(P1, P2, P3); + TriangleP tri2(Q1, Q2, Q3); + + // TODO(louis): MeshCollisionTraversalNode should have its own GJKSolver. + GJKSolver solver(this->request); + + const bool compute_penetration = + this->request.enable_contact || (this->request.security_margin < 0); + Vec3s p1, p2, normal; + DistanceResult distanceResult; + CoalScalar distance = internal::ShapeShapeDistance( + &tri1, this->tf1, &tri2, this->tf2, &solver, compute_penetration, p1, + p2, normal); + + const CoalScalar distToCollision = distance - this->request.security_margin; + + internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result), + distToCollision, p1, p2, normal); + + if (distToCollision <= + this->request.collision_distance_threshold) { // collision + sqrDistLowerBound = 0; + if (this->result->numContacts() < this->request.num_max_contacts) { + this->result->addContact(Contact(this->model1, this->model2, + primitive_id1, primitive_id2, p1, p2, + normal, distance)); + } + } else + sqrDistLowerBound = distToCollision * distToCollision; + } + + Vec3s* vertices1; + Vec3s* vertices2; + + Triangle* tri_indices1; + Triangle* tri_indices2; + + details::RelativeTransformation RT; +}; + +/// @brief Traversal node for collision between two meshes if their underlying +/// BVH node is oriented node (OBB, RSS, OBBRSS, kIOS) +typedef MeshCollisionTraversalNode MeshCollisionTraversalNodeOBB; +typedef MeshCollisionTraversalNode MeshCollisionTraversalNodeRSS; +typedef MeshCollisionTraversalNode MeshCollisionTraversalNodekIOS; +typedef MeshCollisionTraversalNode MeshCollisionTraversalNodeOBBRSS; + +/// @} + +namespace details { +template +struct DistanceTraversalBVDistanceLowerBound_impl { + static CoalScalar run(const BVNode& b1, const BVNode& b2) { + return b1.distance(b2); + } + static CoalScalar run(const Matrix3s& R, const Vec3s& T, const BVNode& b1, + const BVNode& b2) { + return distance(R, T, b1.bv, b2.bv); + } +}; + +template <> +struct DistanceTraversalBVDistanceLowerBound_impl { + static CoalScalar run(const BVNode& b1, const BVNode& b2) { + CoalScalar sqrDistLowerBound; + CollisionRequest request(DISTANCE_LOWER_BOUND, 0); + // request.break_distance = ? + if (b1.overlap(b2, request, sqrDistLowerBound)) { + // TODO A penetration upper bound should be computed. + return -1; + } + return sqrt(sqrDistLowerBound); + } + static CoalScalar run(const Matrix3s& R, const Vec3s& T, + const BVNode& b1, const BVNode& b2) { + CoalScalar sqrDistLowerBound; + CollisionRequest request(DISTANCE_LOWER_BOUND, 0); + // request.break_distance = ? + if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { + // TODO A penetration upper bound should be computed. + return -1; + } + return sqrt(sqrDistLowerBound); + } +}; + +template <> +struct DistanceTraversalBVDistanceLowerBound_impl { + static CoalScalar run(const BVNode& b1, const BVNode& b2) { + CoalScalar sqrDistLowerBound; + CollisionRequest request(DISTANCE_LOWER_BOUND, 0); + // request.break_distance = ? + if (b1.overlap(b2, request, sqrDistLowerBound)) { + // TODO A penetration upper bound should be computed. + return -1; + } + return sqrt(sqrDistLowerBound); + } + static CoalScalar run(const Matrix3s& R, const Vec3s& T, + const BVNode& b1, const BVNode& b2) { + CoalScalar sqrDistLowerBound; + CollisionRequest request(DISTANCE_LOWER_BOUND, 0); + // request.break_distance = ? + if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { + // TODO A penetration upper bound should be computed. + return -1; + } + return sqrt(sqrDistLowerBound); + } +}; +} // namespace details + +/// @addtogroup Traversal_For_Distance +/// @{ + +/// @brief Traversal node for distance computation between BVH models +template +class BVHDistanceTraversalNode : public DistanceTraversalNodeBase { + public: + BVHDistanceTraversalNode() : DistanceTraversalNodeBase() { + model1 = NULL; + model2 = NULL; + + num_bv_tests = 0; + num_leaf_tests = 0; + query_time_seconds = 0.0; + } + + /// @brief Whether the BV node in the first BVH tree is leaf + bool isFirstNodeLeaf(unsigned int b) const { + return model1->getBV(b).isLeaf(); + } + + /// @brief Whether the BV node in the second BVH tree is leaf + bool isSecondNodeLeaf(unsigned int b) const { + return model2->getBV(b).isLeaf(); + } + + /// @brief Determine the traversal order, is the first BVTT subtree better + bool firstOverSecond(unsigned int b1, unsigned int b2) const { + CoalScalar sz1 = model1->getBV(b1).bv.size(); + CoalScalar sz2 = model2->getBV(b2).bv.size(); + + bool l1 = model1->getBV(b1).isLeaf(); + bool l2 = model2->getBV(b2).isLeaf(); + + if (l2 || (!l1 && (sz1 > sz2))) return true; + return false; + } + + /// @brief Obtain the left child of BV node in the first BVH + int getFirstLeftChild(unsigned int b) const { + return model1->getBV(b).leftChild(); + } + + /// @brief Obtain the right child of BV node in the first BVH + int getFirstRightChild(unsigned int b) const { + return model1->getBV(b).rightChild(); + } + + /// @brief Obtain the left child of BV node in the second BVH + int getSecondLeftChild(unsigned int b) const { + return model2->getBV(b).leftChild(); + } + + /// @brief Obtain the right child of BV node in the second BVH + int getSecondRightChild(unsigned int b) const { + return model2->getBV(b).rightChild(); + } + + /// @brief The first BVH model + const BVHModel* model1; + /// @brief The second BVH model + const BVHModel* model2; + + /// @brief statistical information + mutable int num_bv_tests; + mutable int num_leaf_tests; + mutable CoalScalar query_time_seconds; +}; + +/// @brief Traversal node for distance computation between two meshes +template +class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { + public: + enum { + Options = _Options, + RTIsIdentity = _Options & RelativeTransformationIsIdentity + }; + + using BVHDistanceTraversalNode::enable_statistics; + using BVHDistanceTraversalNode::request; + using BVHDistanceTraversalNode::result; + using BVHDistanceTraversalNode::tf1; + using BVHDistanceTraversalNode::model1; + using BVHDistanceTraversalNode::model2; + using BVHDistanceTraversalNode::num_bv_tests; + using BVHDistanceTraversalNode::num_leaf_tests; + + MeshDistanceTraversalNode() : BVHDistanceTraversalNode() { + vertices1 = NULL; + vertices2 = NULL; + tri_indices1 = NULL; + tri_indices2 = NULL; + + rel_err = this->request.rel_err; + abs_err = this->request.abs_err; + } + + void preprocess() { + if (!RTIsIdentity) preprocessOrientedNode(); + } + + void postprocess() { + if (!RTIsIdentity) postprocessOrientedNode(); + } + + /// @brief BV culling test in one BVTT node + CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int b2) const { + if (enable_statistics) num_bv_tests++; + if (RTIsIdentity) + return details::DistanceTraversalBVDistanceLowerBound_impl::run( + model1->getBV(b1), model2->getBV(b2)); + else + return details::DistanceTraversalBVDistanceLowerBound_impl::run( + RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2)); + } + + /// @brief Distance testing between leaves (two triangles) + void leafComputeDistance(unsigned int b1, unsigned int b2) const { + if (this->enable_statistics) this->num_leaf_tests++; + + const BVNode& node1 = this->model1->getBV(b1); + const BVNode& node2 = this->model2->getBV(b2); + + int primitive_id1 = node1.primitiveId(); + int primitive_id2 = node2.primitiveId(); + + const Triangle& tri_id1 = tri_indices1[primitive_id1]; + const Triangle& tri_id2 = tri_indices2[primitive_id2]; + + const Vec3s& t11 = vertices1[tri_id1[0]]; + const Vec3s& t12 = vertices1[tri_id1[1]]; + const Vec3s& t13 = vertices1[tri_id1[2]]; + + const Vec3s& t21 = vertices2[tri_id2[0]]; + const Vec3s& t22 = vertices2[tri_id2[1]]; + const Vec3s& t23 = vertices2[tri_id2[2]]; + + // nearest point pair + Vec3s P1, P2, normal; + + CoalScalar d2; + if (RTIsIdentity) + d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1, + P2); + else + d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, + RT._R(), RT._T(), P1, P2); + CoalScalar d = sqrt(d2); + + this->result->update(d, this->model1, this->model2, primitive_id1, + primitive_id2, P1, P2, normal); + } + + /// @brief Whether the traversal process can stop early + bool canStop(CoalScalar c) const { + if ((c >= this->result->min_distance - abs_err) && + (c * (1 + rel_err) >= this->result->min_distance)) + return true; + return false; + } + + Vec3s* vertices1; + Vec3s* vertices2; + + Triangle* tri_indices1; + Triangle* tri_indices2; + + /// @brief relative and absolute error, default value is 0.01 for both terms + CoalScalar rel_err; + CoalScalar abs_err; + + details::RelativeTransformation RT; + + private: + void preprocessOrientedNode() { + const int init_tri_id1 = 0, init_tri_id2 = 0; + const Triangle& init_tri1 = tri_indices1[init_tri_id1]; + const Triangle& init_tri2 = tri_indices2[init_tri_id2]; + + Vec3s init_tri1_points[3]; + Vec3s init_tri2_points[3]; + + init_tri1_points[0] = vertices1[init_tri1[0]]; + init_tri1_points[1] = vertices1[init_tri1[1]]; + init_tri1_points[2] = vertices1[init_tri1[2]]; + + init_tri2_points[0] = vertices2[init_tri2[0]]; + init_tri2_points[1] = vertices2[init_tri2[1]]; + init_tri2_points[2] = vertices2[init_tri2[2]]; + + Vec3s p1, p2, normal; + CoalScalar distance = sqrt(TriangleDistance::sqrTriDistance( + init_tri1_points[0], init_tri1_points[1], init_tri1_points[2], + init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(), + RT._T(), p1, p2)); + + result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2, + normal); + } + void postprocessOrientedNode() { + /// the points obtained by triDistance are not in world space: both are in + /// object1's local coordinate system, so we need to convert them into the + /// world space. + if (request.enable_nearest_points && (result->o1 == model1) && + (result->o2 == model2)) { + result->nearest_points[0] = tf1.transform(result->nearest_points[0]); + result->nearest_points[1] = tf1.transform(result->nearest_points[1]); + } + } +}; + +/// @brief Traversal node for distance computation between two meshes if their +/// underlying BVH node is oriented node (RSS, OBBRSS, kIOS) +typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeRSS; +typedef MeshDistanceTraversalNode MeshDistanceTraversalNodekIOS; +typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeOBBRSS; + +/// @} + +/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to +/// be transformed +namespace details { + +template +inline const Matrix3s& getBVAxes(const BV& bv) { + return bv.axes; +} + +template <> +inline const Matrix3s& getBVAxes(const OBBRSS& bv) { + return bv.obb.axes; +} + +} // namespace details + +} // namespace coal + +/// @endcond + +#endif diff --git a/include/coal/internal/traversal_node_hfield_shape.h b/include/coal/internal/traversal_node_hfield_shape.h new file mode 100644 index 000000000..20e6202c2 --- /dev/null +++ b/include/coal/internal/traversal_node_hfield_shape.h @@ -0,0 +1,756 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021-2024, INRIA. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_TRAVERSAL_NODE_HFIELD_SHAPE_H +#define COAL_TRAVERSAL_NODE_HFIELD_SHAPE_H + +/// @cond INTERNAL + +#include "coal/collision_data.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/internal/shape_shape_func.h" +#include "coal/internal/traversal_node_base.h" +#include "coal/internal/traversal.h" +#include "coal/internal/intersect.h" +#include "coal/hfield.h" +#include "coal/shape/convex.h" + +namespace coal { + +/// @addtogroup Traversal_For_Collision +/// @{ + +namespace details { +template +Convex buildConvexQuadrilateral(const HFNode& node, + const HeightField& model) { + const MatrixXs& heights = model.getHeights(); + const VecXs& x_grid = model.getXGrid(); + const VecXs& y_grid = model.getYGrid(); + + const CoalScalar min_height = model.getMinHeight(); + + const CoalScalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], + y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; + const Eigen::Block cell = + heights.block<2, 2>(node.y_id, node.x_id); + + assert(cell.maxCoeff() > min_height && + "max_height is lower than min_height"); // Check whether the geometry + // is degenerated + + std::shared_ptr> pts(new std::vector({ + Vec3s(x0, y0, min_height), + Vec3s(x0, y1, min_height), + Vec3s(x1, y1, min_height), + Vec3s(x1, y0, min_height), + Vec3s(x0, y0, cell(0, 0)), + Vec3s(x0, y1, cell(1, 0)), + Vec3s(x1, y1, cell(1, 1)), + Vec3s(x1, y0, cell(0, 1)), + })); + + std::shared_ptr> polygons( + new std::vector(6)); + (*polygons)[0].set(0, 3, 2, 1); // x+ side + (*polygons)[1].set(0, 1, 5, 4); // y- side + (*polygons)[2].set(1, 2, 6, 5); // x- side + (*polygons)[3].set(2, 3, 7, 6); // y+ side + (*polygons)[4].set(3, 0, 4, 7); // z- side + (*polygons)[5].set(4, 5, 6, 7); // z+ side + + return Convex(pts, // points + 8, // num points + polygons, + 6 // number of polygons + ); +} + +enum class FaceOrientationConvexPart1 { + BOTTOM = 0, + TOP = 1, + WEST = 2, + SOUTH_EAST = 4, + NORTH = 8, +}; + +enum class FaceOrientationConvexPart2 { + BOTTOM = 0, + TOP = 1, + SOUTH = 2, + NORTH_WEST = 4, + EAST = 8, +}; + +template +void buildConvexTriangles(const HFNode& node, const HeightField& model, + Convex& convex1, int& convex1_active_faces, + Convex& convex2, + int& convex2_active_faces) { + const MatrixXs& heights = model.getHeights(); + const VecXs& x_grid = model.getXGrid(); + const VecXs& y_grid = model.getYGrid(); + + const CoalScalar min_height = model.getMinHeight(); + + const CoalScalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], + y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; + const CoalScalar max_height = node.max_height; + const Eigen::Block cell = + heights.block<2, 2>(node.y_id, node.x_id); + + const int contact_active_faces = node.contact_active_faces; + convex1_active_faces = 0; + convex2_active_faces = 0; + + typedef HFNodeBase::FaceOrientation FaceOrientation; + + if (contact_active_faces & FaceOrientation::TOP) { + convex1_active_faces |= int(details::FaceOrientationConvexPart1::TOP); + convex2_active_faces |= int(details::FaceOrientationConvexPart2::TOP); + } + + if (contact_active_faces & FaceOrientation::BOTTOM) { + convex1_active_faces |= int(details::FaceOrientationConvexPart1::BOTTOM); + convex2_active_faces |= int(details::FaceOrientationConvexPart2::BOTTOM); + } + + // Specific orientation for Convex1 + if (contact_active_faces & FaceOrientation::WEST) { + convex1_active_faces |= int(details::FaceOrientationConvexPart1::WEST); + } + + if (contact_active_faces & FaceOrientation::NORTH) { + convex1_active_faces |= int(details::FaceOrientationConvexPart1::NORTH); + } + + // Specific orientation for Convex2 + if (contact_active_faces & FaceOrientation::EAST) { + convex2_active_faces |= int(details::FaceOrientationConvexPart2::EAST); + } + + if (contact_active_faces & FaceOrientation::SOUTH) { + convex2_active_faces |= int(details::FaceOrientationConvexPart2::SOUTH); + } + + assert(max_height > min_height && + "max_height is lower than min_height"); // Check whether the geometry + // is degenerated + COAL_UNUSED_VARIABLE(max_height); + + { + std::shared_ptr> pts(new std::vector({ + Vec3s(x0, y0, min_height), // A + Vec3s(x0, y1, min_height), // B + Vec3s(x1, y0, min_height), // C + Vec3s(x0, y0, cell(0, 0)), // D + Vec3s(x0, y1, cell(1, 0)), // E + Vec3s(x1, y0, cell(0, 1)), // F + })); + + std::shared_ptr> triangles( + new std::vector(8)); + (*triangles)[0].set(0, 2, 1); // bottom + (*triangles)[1].set(3, 4, 5); // top + (*triangles)[2].set(0, 1, 3); // West 1 + (*triangles)[3].set(3, 1, 4); // West 2 + (*triangles)[4].set(1, 2, 5); // South-East 1 + (*triangles)[5].set(1, 5, 4); // South-East 1 + (*triangles)[6].set(0, 5, 2); // North 1 + (*triangles)[7].set(5, 0, 3); // North 2 + + convex1.set(pts, // points + 6, // num points + triangles, + 8 // number of polygons + ); + } + + { + std::shared_ptr> pts(new std::vector({ + Vec3s(x0, y1, min_height), // A + Vec3s(x1, y1, min_height), // B + Vec3s(x1, y0, min_height), // C + Vec3s(x0, y1, cell(1, 0)), // D + Vec3s(x1, y1, cell(1, 1)), // E + Vec3s(x1, y0, cell(0, 1)), // F + })); + + std::shared_ptr> triangles( + new std::vector(8)); + (*triangles)[0].set(2, 1, 0); // bottom + (*triangles)[1].set(3, 4, 5); // top + (*triangles)[2].set(0, 1, 3); // South 1 + (*triangles)[3].set(3, 1, 4); // South 2 + (*triangles)[4].set(0, 5, 2); // North West 1 + (*triangles)[5].set(0, 3, 5); // North West 2 + (*triangles)[6].set(1, 2, 5); // East 1 + (*triangles)[7].set(4, 1, 2); // East 2 + + convex2.set(pts, // points + 6, // num points + triangles, + 8 // number of polygons + ); + } +} + +inline Vec3s projectTriangle(const Vec3s& pointA, const Vec3s& pointB, + const Vec3s& pointC, const Vec3s& point) { + const Project::ProjectResult result = + Project::projectTriangle(pointA, pointB, pointC, point); + Vec3s res = result.parameterization[0] * pointA + + result.parameterization[1] * pointB + + result.parameterization[2] * pointC; + + return res; +} + +inline Vec3s projectTetrahedra(const Vec3s& pointA, const Vec3s& pointB, + const Vec3s& pointC, const Vec3s& pointD, + const Vec3s& point) { + const Project::ProjectResult result = + Project::projectTetrahedra(pointA, pointB, pointC, pointD, point); + Vec3s res = result.parameterization[0] * pointA + + result.parameterization[1] * pointB + + result.parameterization[2] * pointC + + result.parameterization[3] * pointD; + + return res; +} + +inline Vec3s computeTriangleNormal(const Triangle& triangle, + const std::vector& points) { + const Vec3s pointA = points[triangle[0]]; + const Vec3s pointB = points[triangle[1]]; + const Vec3s pointC = points[triangle[2]]; + + const Vec3s normal = (pointB - pointA).cross(pointC - pointA).normalized(); + assert(!normal.array().isNaN().any() && "normal is ill-defined"); + + return normal; +} + +inline Vec3s projectPointOnTriangle(const Vec3s& contact_point, + const Triangle& triangle, + const std::vector& points) { + const Vec3s pointA = points[triangle[0]]; + const Vec3s pointB = points[triangle[1]]; + const Vec3s pointC = points[triangle[2]]; + + const Vec3s contact_point_projected = + projectTriangle(pointA, pointB, pointC, contact_point); + + return contact_point_projected; +} + +inline CoalScalar distanceContactPointToTriangle( + const Vec3s& contact_point, const Triangle& triangle, + const std::vector& points) { + const Vec3s contact_point_projected = + projectPointOnTriangle(contact_point, triangle, points); + return (contact_point_projected - contact_point).norm(); +} + +inline CoalScalar distanceContactPointToFace(const size_t face_id, + const Vec3s& contact_point, + const Convex& convex, + size_t& closest_face_id) { + assert((face_id >= 0 && face_id < 8) && "face_id should be in [0;7]"); + + const std::vector& points = *(convex.points); + if (face_id <= 1) { + const Triangle& triangle = (*(convex.polygons))[face_id]; + closest_face_id = face_id; + return distanceContactPointToTriangle(contact_point, triangle, points); + } else { + const Triangle& triangle1 = (*(convex.polygons))[face_id]; + const CoalScalar distance_to_triangle1 = + distanceContactPointToTriangle(contact_point, triangle1, points); + + const Triangle& triangle2 = (*(convex.polygons))[face_id + 1]; + const CoalScalar distance_to_triangle2 = + distanceContactPointToTriangle(contact_point, triangle2, points); + + if (distance_to_triangle1 > distance_to_triangle2) { + closest_face_id = face_id + 1; + return distance_to_triangle2; + } else { + closest_face_id = face_id; + return distance_to_triangle1; + } + } +} + +template +bool binCorrection(const Convex& convex, + const int convex_active_faces, const Shape& shape, + const Transform3s& shape_pose, CoalScalar& distance, + Vec3s& contact_1, Vec3s& contact_2, Vec3s& normal, + Vec3s& face_normal, const bool is_collision) { + const CoalScalar prec = 1e-12; + const std::vector& points = *(convex.points); + + bool hfield_witness_is_on_bin_side = true; + + // int closest_face_id_bottom_face = -1; + // int closest_face_id_top_face = -1; + + std::vector active_faces; + active_faces.reserve(5); + active_faces.push_back(0); + active_faces.push_back(1); + + if (convex_active_faces & 2) active_faces.push_back(2); + if (convex_active_faces & 4) active_faces.push_back(4); + if (convex_active_faces & 8) active_faces.push_back(6); + + Triangle face_triangle; + CoalScalar shortest_distance_to_face = + (std::numeric_limits::max)(); + face_normal = normal; + for (const size_t active_face : active_faces) { + size_t closest_face_id; + const CoalScalar distance_to_face = distanceContactPointToFace( + active_face, contact_1, convex, closest_face_id); + + const bool contact_point_is_on_face = distance_to_face <= prec; + if (contact_point_is_on_face) { + hfield_witness_is_on_bin_side = false; + face_triangle = (*(convex.polygons))[closest_face_id]; + shortest_distance_to_face = distance_to_face; + break; + } else if (distance_to_face < shortest_distance_to_face) { + face_triangle = (*(convex.polygons))[closest_face_id]; + shortest_distance_to_face = distance_to_face; + } + } + + // We correct only if there is a collision with the bin + if (is_collision) { + if (!face_triangle.isValid()) + COAL_THROW_PRETTY("face_triangle is not initialized", std::logic_error); + + const Vec3s face_pointA = points[face_triangle[0]]; + face_normal = computeTriangleNormal(face_triangle, points); + + int hint = 0; + // Since we compute the support manually, we need to take into account the + // sphere swept radius of the shape. + // TODO: take into account the swept-sphere radius of the bin. + const Vec3s _support = getSupport( + &shape, -shape_pose.rotation().transpose() * face_normal, hint); + const Vec3s support = + shape_pose.rotation() * _support + shape_pose.translation(); + + // Project support into the inclined bin having triangle + const CoalScalar offset_plane = face_normal.dot(face_pointA); + const Plane projection_plane(face_normal, offset_plane); + const CoalScalar distance_support_projection_plane = + projection_plane.signedDistance(support); + + const Vec3s projected_support = + support - distance_support_projection_plane * face_normal; + + // We need now to project the projected in the triangle shape + contact_1 = + projectPointOnTriangle(projected_support, face_triangle, points); + contact_2 = contact_1 + distance_support_projection_plane * face_normal; + normal = face_normal; + distance = -std::fabs(distance_support_projection_plane); + } + + return hfield_witness_is_on_bin_side; +} + +template +bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request, + const Convex& convex1, + const int convex1_active_faces, + const Convex& convex2, + const int convex2_active_faces, const Transform3s& tf1, + const Shape& shape, const Transform3s& tf2, + CoalScalar& distance, Vec3s& c1, Vec3s& c2, Vec3s& normal, + Vec3s& normal_top, bool& hfield_witness_is_on_bin_side) { + enum { RTIsIdentity = Options & RelativeTransformationIsIdentity }; + + const Transform3s Id; + // The solver `nsolver` has already been set up by the collision request + // `request`. If GJK early stopping is enabled through `request`, it will be + // used. + // The only thing we need to make sure is that in case of collision, the + // penetration information is computed (as we do bins comparison). + const bool compute_penetration = true; + Vec3s contact1_1, contact1_2, contact2_1, contact2_2; + Vec3s normal1, normal1_top, normal2, normal2_top; + CoalScalar distance1, distance2; + + if (RTIsIdentity) { + distance1 = internal::ShapeShapeDistance, Shape>( + &convex1, Id, &shape, tf2, nsolver, compute_penetration, contact1_1, + contact1_2, normal1); + } else { + distance1 = internal::ShapeShapeDistance, Shape>( + &convex1, tf1, &shape, tf2, nsolver, compute_penetration, contact1_1, + contact1_2, normal1); + } + bool collision1 = (distance1 - request.security_margin <= + request.collision_distance_threshold); + + bool hfield_witness_is_on_bin_side1 = + binCorrection(convex1, convex1_active_faces, shape, tf2, distance1, + contact1_1, contact1_2, normal1, normal1_top, collision1); + + if (RTIsIdentity) { + distance2 = internal::ShapeShapeDistance, Shape>( + &convex2, Id, &shape, tf2, nsolver, compute_penetration, contact2_1, + contact2_2, normal2); + } else { + distance2 = internal::ShapeShapeDistance, Shape>( + &convex2, tf1, &shape, tf2, nsolver, compute_penetration, contact2_1, + contact2_2, normal2); + } + bool collision2 = (distance2 - request.security_margin <= + request.collision_distance_threshold); + + bool hfield_witness_is_on_bin_side2 = + binCorrection(convex2, convex2_active_faces, shape, tf2, distance2, + contact2_1, contact2_2, normal2, normal2_top, collision2); + + if (collision1 && collision2) { + if (distance1 > distance2) // switch values + { + distance = distance2; + c1 = contact2_1; + c2 = contact2_2; + normal = normal2; + normal_top = normal2_top; + hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2; + } else { + distance = distance1; + c1 = contact1_1; + c2 = contact1_2; + normal = normal1; + normal_top = normal1_top; + hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1; + } + return true; + } else if (collision1) { + distance = distance1; + c1 = contact1_1; + c2 = contact1_2; + normal = normal1; + normal_top = normal1_top; + hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1; + return true; + } else if (collision2) { + distance = distance2; + c1 = contact2_1; + c2 = contact2_2; + normal = normal2; + normal_top = normal2_top; + hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2; + return true; + } + + if (distance1 > distance2) // switch values + { + distance = distance2; + c1 = contact2_1; + c2 = contact2_2; + normal = normal2; + normal_top = normal2_top; + hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2; + } else { + distance = distance1; + c1 = contact1_1; + c2 = contact1_2; + normal = normal1; + normal_top = normal1_top; + hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1; + } + return false; +} + +} // namespace details + +/// @brief Traversal node for collision between height field and shape +template +class HeightFieldShapeCollisionTraversalNode + : public CollisionTraversalNodeBase { + public: + typedef CollisionTraversalNodeBase Base; + typedef Eigen::Array Array2d; + + enum { + Options = _Options, + RTIsIdentity = _Options & RelativeTransformationIsIdentity + }; + + HeightFieldShapeCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { + model1 = NULL; + model2 = NULL; + + num_bv_tests = 0; + num_leaf_tests = 0; + query_time_seconds = 0.0; + + nsolver = NULL; + count = 0; + } + + /// @brief Whether the BV node in the first BVH tree is leaf + bool isFirstNodeLeaf(unsigned int b) const { + return model1->getBV(b).isLeaf(); + } + + /// @brief Obtain the left child of BV node in the first BVH + int getFirstLeftChild(unsigned int b) const { + return static_cast(model1->getBV(b).leftChild()); + } + + /// @brief Obtain the right child of BV node in the first BVH + int getFirstRightChild(unsigned int b) const { + return static_cast(model1->getBV(b).rightChild()); + } + + /// test between BV b1 and shape + /// @param b1 BV to test, + /// @retval sqrDistLowerBound square of a lower bound of the minimal + /// distance between bounding volumes. + /// @brief BV culling test in one BVTT node + bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, + CoalScalar& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_bv_tests++; + + bool disjoint; + if (RTIsIdentity) { + assert(false && "must never happened"); + disjoint = !this->model1->getBV(b1).bv.overlap( + this->model2_bv, this->request, sqrDistLowerBound); + } else { + disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), + this->model1->getBV(b1).bv, this->model2_bv, + this->request, sqrDistLowerBound); + } + + if (disjoint) + internal::updateDistanceLowerBoundFromBV(this->request, *this->result, + sqrDistLowerBound); + + assert(!disjoint || sqrDistLowerBound > 0); + return disjoint; + } + + /// @brief Intersection testing between leaves (one Convex and one shape) + void leafCollides(unsigned int b1, unsigned int /*b2*/, + CoalScalar& sqrDistLowerBound) const { + count++; + if (this->enable_statistics) this->num_leaf_tests++; + const HFNode& node = this->model1->getBV(b1); + + // Split quadrilateral primitives into two convex shapes corresponding to + // polyhedron with triangular bases. This is essential to keep the convexity + + // typedef Convex ConvexQuadrilateral; + // const ConvexQuadrilateral convex = + // details::buildConvexQuadrilateral(node,*this->model1); + + typedef Convex ConvexTriangle; + ConvexTriangle convex1, convex2; + int convex1_active_faces, convex2_active_faces; + // TODO: inherit from hfield's inflation here + details::buildConvexTriangles(node, *this->model1, convex1, + convex1_active_faces, convex2, + convex2_active_faces); + + // Compute aabb_local for BoundingVolumeGuess case in the GJK solver + if (nsolver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { + convex1.computeLocalAABB(); + convex2.computeLocalAABB(); + } + + CoalScalar distance; + // Vec3s contact_point, normal; + Vec3s c1, c2, normal, normal_face; + bool hfield_witness_is_on_bin_side; + + bool collision = details::shapeDistance( + nsolver, this->request, convex1, convex1_active_faces, convex2, + convex2_active_faces, this->tf1, *(this->model2), this->tf2, distance, + c1, c2, normal, normal_face, hfield_witness_is_on_bin_side); + + CoalScalar distToCollision = distance - this->request.security_margin; + if (distToCollision <= this->request.collision_distance_threshold) { + sqrDistLowerBound = 0; + if (this->result->numContacts() < this->request.num_max_contacts) { + if (normal_face.isApprox(normal) && + (collision || !hfield_witness_is_on_bin_side)) { + this->result->addContact(Contact(this->model1, this->model2, (int)b1, + (int)Contact::NONE, c1, c2, normal, + distance)); + assert(this->result->isCollision()); + } + } + } else + sqrDistLowerBound = distToCollision * distToCollision; + + // const Vec3s c1 = contact_point - distance * 0.5 * normal; + // const Vec3s c2 = contact_point + distance * 0.5 * normal; + internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result, + distToCollision, c1, c2, normal); + + assert(this->result->isCollision() || sqrDistLowerBound > 0); + } + + const GJKSolver* nsolver; + + const HeightField* model1; + const S* model2; + BV model2_bv; + + mutable int num_bv_tests; + mutable int num_leaf_tests; + mutable CoalScalar query_time_seconds; + mutable int count; +}; + +/// @} + +/// @addtogroup Traversal_For_Distance +/// @{ + +/// @brief Traversal node for distance between height field and shape +template +class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase { + public: + typedef DistanceTraversalNodeBase Base; + + enum { + Options = _Options, + RTIsIdentity = _Options & RelativeTransformationIsIdentity + }; + + HeightFieldShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { + model1 = NULL; + model2 = NULL; + + num_leaf_tests = 0; + query_time_seconds = 0.0; + + rel_err = 0; + abs_err = 0; + nsolver = NULL; + } + + /// @brief Whether the BV node in the first BVH tree is leaf + bool isFirstNodeLeaf(unsigned int b) const { + return model1->getBV(b).isLeaf(); + } + + /// @brief Obtain the left child of BV node in the first BVH + int getFirstLeftChild(unsigned int b) const { + return model1->getBV(b).leftChild(); + } + + /// @brief Obtain the right child of BV node in the first BVH + int getFirstRightChild(unsigned int b) const { + return model1->getBV(b).rightChild(); + } + + /// @brief BV culling test in one BVTT node + CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { + return model1->getBV(b1).bv.distance( + model2_bv); // TODO(jcarpent): tf1 is not taken into account here. + } + + /// @brief Distance testing between leaves (one bin of the height field and + /// one shape) + /// TODO(louis): deal with Hfield-Shape distance just like in Hfield-Shape + /// collision (bin correction etc). + void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const { + if (this->enable_statistics) this->num_leaf_tests++; + + const BVNode& node = this->model1->getBV(b1); + + typedef Convex ConvexQuadrilateral; + const ConvexQuadrilateral convex = + details::buildConvexQuadrilateral(node, *this->model1); + + Vec3s p1, p2, normal; + const CoalScalar distance = + internal::ShapeShapeDistance( + &convex, this->tf1, this->model2, this->tf2, this->nsolver, + this->request.enable_signed_distance, p1, p2, normal); + + this->result->update(distance, this->model1, this->model2, b1, + DistanceResult::NONE, p1, p2, normal); + } + + /// @brief Whether the traversal process can stop early + bool canStop(CoalScalar c) const { + if ((c >= this->result->min_distance - abs_err) && + (c * (1 + rel_err) >= this->result->min_distance)) + return true; + return false; + } + + CoalScalar rel_err; + CoalScalar abs_err; + + const GJKSolver* nsolver; + + const HeightField* model1; + const S* model2; + BV model2_bv; + + mutable int num_bv_tests; + mutable int num_leaf_tests; + mutable CoalScalar query_time_seconds; +}; + +/// @} + +} // namespace coal + +/// @endcond + +#endif diff --git a/include/coal/internal/traversal_node_octree.h b/include/coal/internal/traversal_node_octree.h new file mode 100644 index 000000000..2f9da5f07 --- /dev/null +++ b/include/coal/internal/traversal_node_octree.h @@ -0,0 +1,1369 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2022-2023, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_TRAVERSAL_NODE_OCTREE_H +#define COAL_TRAVERSAL_NODE_OCTREE_H + +/// @cond INTERNAL + +#include "coal/collision_data.h" +#include "coal/internal/traversal_node_base.h" +#include "coal/internal/traversal_node_hfield_shape.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/hfield.h" +#include "coal/octree.h" +#include "coal/BVH/BVH_model.h" +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/internal/shape_shape_func.h" + +namespace coal { + +/// @brief Algorithms for collision related with octree +class COAL_DLLAPI OcTreeSolver { + private: + const GJKSolver* solver; + + mutable const CollisionRequest* crequest; + mutable const DistanceRequest* drequest; + + mutable CollisionResult* cresult; + mutable DistanceResult* dresult; + + public: + OcTreeSolver(const GJKSolver* solver_) + : solver(solver_), + crequest(NULL), + drequest(NULL), + cresult(NULL), + dresult(NULL) {} + + /// @brief collision between two octrees + void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2, + const Transform3s& tf1, const Transform3s& tf2, + const CollisionRequest& request_, + CollisionResult& result_) const { + crequest = &request_; + cresult = &result_; + + OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, + tree2->getRoot(), tree2->getRootBV(), tf1, tf2); + } + + /// @brief distance between two octrees + void OcTreeDistance(const OcTree* tree1, const OcTree* tree2, + const Transform3s& tf1, const Transform3s& tf2, + const DistanceRequest& request_, + DistanceResult& result_) const { + drequest = &request_; + dresult = &result_; + + OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, + tree2->getRoot(), tree2->getRootBV(), tf1, tf2); + } + + /// @brief collision between octree and mesh + template + void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel* tree2, + const Transform3s& tf1, const Transform3s& tf2, + const CollisionRequest& request_, + CollisionResult& result_) const { + crequest = &request_; + cresult = &result_; + + OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), + tree2, 0, tf1, tf2); + } + + /// @brief distance between octree and mesh + template + void OcTreeMeshDistance(const OcTree* tree1, const BVHModel* tree2, + const Transform3s& tf1, const Transform3s& tf2, + const DistanceRequest& request_, + DistanceResult& result_) const { + drequest = &request_; + dresult = &result_; + + OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), + tree2, 0, tf1, tf2); + } + + /// @brief collision between mesh and octree + template + void MeshOcTreeIntersect(const BVHModel* tree1, const OcTree* tree2, + const Transform3s& tf1, const Transform3s& tf2, + const CollisionRequest& request_, + CollisionResult& result_) const + + { + crequest = &request_; + cresult = &result_; + + OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(), + tree1, 0, tf2, tf1); + } + + /// @brief distance between mesh and octree + template + void MeshOcTreeDistance(const BVHModel* tree1, const OcTree* tree2, + const Transform3s& tf1, const Transform3s& tf2, + const DistanceRequest& request_, + DistanceResult& result_) const { + drequest = &request_; + dresult = &result_; + + OcTreeMeshDistanceRecurse(tree1, 0, tree2, tree2->getRoot(), + tree2->getRootBV(), tf1, tf2); + } + + template + void OcTreeHeightFieldIntersect( + const OcTree* tree1, const HeightField* tree2, const Transform3s& tf1, + const Transform3s& tf2, const CollisionRequest& request_, + CollisionResult& result_, CoalScalar& sqrDistLowerBound) const { + crequest = &request_; + cresult = &result_; + + OcTreeHeightFieldIntersectRecurse(tree1, tree1->getRoot(), + tree1->getRootBV(), tree2, 0, tf1, tf2, + sqrDistLowerBound); + } + + template + void HeightFieldOcTreeIntersect(const HeightField* tree1, + const OcTree* tree2, const Transform3s& tf1, + const Transform3s& tf2, + const CollisionRequest& request_, + CollisionResult& result_, + CoalScalar& sqrDistLowerBound) const { + crequest = &request_; + cresult = &result_; + + HeightFieldOcTreeIntersectRecurse(tree1, 0, tree2, tree2->getRoot(), + tree2->getRootBV(), tf1, tf2, + sqrDistLowerBound); + } + + /// @brief collision between octree and shape + template + void OcTreeShapeIntersect(const OcTree* tree, const S& s, + const Transform3s& tf1, const Transform3s& tf2, + const CollisionRequest& request_, + CollisionResult& result_) const { + crequest = &request_; + cresult = &result_; + + AABB bv2; + computeBV(s, Transform3s(), bv2); + OBB obb2; + convertBV(bv2, tf2, obb2); + OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s, + obb2, tf1, tf2); + } + + /// @brief collision between shape and octree + template + void ShapeOcTreeIntersect(const S& s, const OcTree* tree, + const Transform3s& tf1, const Transform3s& tf2, + const CollisionRequest& request_, + CollisionResult& result_) const { + crequest = &request_; + cresult = &result_; + + AABB bv1; + computeBV(s, Transform3s(), bv1); + OBB obb1; + convertBV(bv1, tf1, obb1); + OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s, + obb1, tf2, tf1); + } + + /// @brief distance between octree and shape + template + void OcTreeShapeDistance(const OcTree* tree, const S& s, + const Transform3s& tf1, const Transform3s& tf2, + const DistanceRequest& request_, + DistanceResult& result_) const { + drequest = &request_; + dresult = &result_; + + AABB aabb2; + computeBV(s, tf2, aabb2); + OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s, + aabb2, tf1, tf2); + } + + /// @brief distance between shape and octree + template + void ShapeOcTreeDistance(const S& s, const OcTree* tree, + const Transform3s& tf1, const Transform3s& tf2, + const DistanceRequest& request_, + DistanceResult& result_) const { + drequest = &request_; + dresult = &result_; + + AABB aabb1; + computeBV(s, tf1, aabb1); + OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s, + aabb1, tf2, tf1); + } + + private: + template + bool OcTreeShapeDistanceRecurse(const OcTree* tree1, + const OcTree::OcTreeNode* root1, + const AABB& bv1, const S& s, + const AABB& aabb2, const Transform3s& tf1, + const Transform3s& tf2) const { + if (!tree1->nodeHasChildren(root1)) { + if (tree1->isNodeOccupied(root1)) { + Box box; + Transform3s box_tf; + constructBox(bv1, tf1, box, box_tf); + + if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { + box.computeLocalAABB(); + } + + Vec3s p1, p2, normal; + const CoalScalar distance = internal::ShapeShapeDistance( + &box, box_tf, &s, tf2, this->solver, + this->drequest->enable_signed_distance, p1, p2, normal); + + this->dresult->update(distance, tree1, &s, + (int)(root1 - tree1->getRoot()), + DistanceResult::NONE, p1, p2, normal); + + return drequest->isSatisfied(*dresult); + } else + return false; + } + + if (!tree1->isNodeOccupied(root1)) return false; + + for (unsigned int i = 0; i < 8; ++i) { + if (tree1->nodeChildExists(root1, i)) { + const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); + AABB child_bv; + computeChildBV(bv1, i, child_bv); + + AABB aabb1; + convertBV(child_bv, tf1, aabb1); + CoalScalar d = aabb1.distance(aabb2); + if (d < dresult->min_distance) { + if (OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, + tf2)) + return true; + } + } + } + + return false; + } + + template + bool OcTreeShapeIntersectRecurse(const OcTree* tree1, + const OcTree::OcTreeNode* root1, + const AABB& bv1, const S& s, const OBB& obb2, + const Transform3s& tf1, + const Transform3s& tf2) const { + // Empty OcTree is considered free. + if (!root1) return false; + + /// stop when 1) bounding boxes of two objects not overlap; OR + /// 2) at least of one the nodes is free; OR + /// 2) (two uncertain nodes or one node occupied and one node + /// uncertain) AND cost not required + if (tree1->isNodeFree(root1)) + return false; + else if ((tree1->isNodeUncertain(root1) || s.isUncertain())) + return false; + else { + OBB obb1; + convertBV(bv1, tf1, obb1); + CoalScalar sqrDistLowerBound; + if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { + internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, + sqrDistLowerBound); + return false; + } + } + + if (!tree1->nodeHasChildren(root1)) { + assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. + + Box box; + Transform3s box_tf; + constructBox(bv1, tf1, box, box_tf); + if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { + box.computeLocalAABB(); + } + + bool contactNotAdded = + (cresult->numContacts() >= crequest->num_max_contacts); + std::size_t ncontact = ShapeShapeCollide( + &box, box_tf, &s, tf2, solver, *crequest, *cresult); + assert(ncontact == 0 || ncontact == 1); + if (!contactNotAdded && ncontact == 1) { + // Update contact information. + const Contact& c = cresult->getContact(cresult->numContacts() - 1); + cresult->setContact( + cresult->numContacts() - 1, + Contact(tree1, c.o2, static_cast(root1 - tree1->getRoot()), + c.b2, c.pos, c.normal, c.penetration_depth)); + } + + // no need to call `internal::updateDistanceLowerBoundFromLeaf` here + // as it is already done internally in `ShapeShapeCollide` above. + return crequest->isSatisfied(*cresult); + } + + for (unsigned int i = 0; i < 8; ++i) { + if (tree1->nodeChildExists(root1, i)) { + const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); + AABB child_bv; + computeChildBV(bv1, i, child_bv); + + if (OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, + tf2)) + return true; + } + } + + return false; + } + + template + bool OcTreeMeshDistanceRecurse(const OcTree* tree1, + const OcTree::OcTreeNode* root1, + const AABB& bv1, const BVHModel* tree2, + unsigned int root2, const Transform3s& tf1, + const Transform3s& tf2) const { + if (!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) { + if (tree1->isNodeOccupied(root1)) { + Box box; + Transform3s box_tf; + constructBox(bv1, tf1, box, box_tf); + + if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { + box.computeLocalAABB(); + } + + size_t primitive_id = + static_cast(tree2->getBV(root2).primitiveId()); + const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id]; + const TriangleP tri((*(tree2->vertices))[tri_id[0]], + (*(tree2->vertices))[tri_id[1]], + (*(tree2->vertices))[tri_id[2]]); + + Vec3s p1, p2, normal; + const CoalScalar distance = + internal::ShapeShapeDistance( + &box, box_tf, &tri, tf2, this->solver, + this->drequest->enable_signed_distance, p1, p2, normal); + + this->dresult->update(distance, tree1, tree2, + (int)(root1 - tree1->getRoot()), + static_cast(primitive_id), p1, p2, normal); + + return this->drequest->isSatisfied(*dresult); + } else + return false; + } + + if (!tree1->isNodeOccupied(root1)) return false; + + if (tree2->getBV(root2).isLeaf() || + (tree1->nodeHasChildren(root1) && + (bv1.size() > tree2->getBV(root2).bv.size()))) { + for (unsigned int i = 0; i < 8; ++i) { + if (tree1->nodeChildExists(root1, i)) { + const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); + AABB child_bv; + computeChildBV(bv1, i, child_bv); + + CoalScalar d; + AABB aabb1, aabb2; + convertBV(child_bv, tf1, aabb1); + convertBV(tree2->getBV(root2).bv, tf2, aabb2); + d = aabb1.distance(aabb2); + + if (d < dresult->min_distance) { + if (OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, + tf1, tf2)) + return true; + } + } + } + } else { + CoalScalar d; + AABB aabb1, aabb2; + convertBV(bv1, tf1, aabb1); + unsigned int child = (unsigned int)tree2->getBV(root2).leftChild(); + convertBV(tree2->getBV(child).bv, tf2, aabb2); + d = aabb1.distance(aabb2); + + if (d < dresult->min_distance) { + if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, + tf2)) + return true; + } + + child = (unsigned int)tree2->getBV(root2).rightChild(); + convertBV(tree2->getBV(child).bv, tf2, aabb2); + d = aabb1.distance(aabb2); + + if (d < dresult->min_distance) { + if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, + tf2)) + return true; + } + } + + return false; + } + + /// \return True if the request is satisfied. + template + bool OcTreeMeshIntersectRecurse(const OcTree* tree1, + const OcTree::OcTreeNode* root1, + const AABB& bv1, const BVHModel* tree2, + unsigned int root2, const Transform3s& tf1, + const Transform3s& tf2) const { + // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The + // code in this if(!root1) did not output anything so the empty OcTree is + // considered free. Should an empty OcTree be considered free ? + + // Empty OcTree is considered free. + if (!root1) return false; + BVNode const& bvn2 = tree2->getBV(root2); + + /// stop when 1) bounding boxes of two objects not overlap; OR + /// 2) at least one of the nodes is free; OR + /// 2) (two uncertain nodes OR one node occupied and one node + /// uncertain) AND cost not required + if (tree1->isNodeFree(root1)) + return false; + else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain())) + return false; + else { + OBB obb1, obb2; + convertBV(bv1, tf1, obb1); + convertBV(bvn2.bv, tf2, obb2); + CoalScalar sqrDistLowerBound; + if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { + internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, + sqrDistLowerBound); + return false; + } + } + + // Check if leaf collides. + if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) { + assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. + Box box; + Transform3s box_tf; + constructBox(bv1, tf1, box, box_tf); + if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { + box.computeLocalAABB(); + } + + size_t primitive_id = static_cast(bvn2.primitiveId()); + const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id]; + const TriangleP tri((*(tree2->vertices))[tri_id[0]], + (*(tree2->vertices))[tri_id[1]], + (*(tree2->vertices))[tri_id[2]]); + + // When reaching this point, `this->solver` has already been set up + // by the CollisionRequest `this->crequest`. + // The only thing we need to (and can) pass to `ShapeShapeDistance` is + // whether or not penetration information is should be computed in case of + // collision. + const bool compute_penetration = this->crequest->enable_contact || + (this->crequest->security_margin < 0); + Vec3s c1, c2, normal; + const CoalScalar distance = internal::ShapeShapeDistance( + &box, box_tf, &tri, tf2, this->solver, compute_penetration, c1, c2, + normal); + const CoalScalar distToCollision = + distance - this->crequest->security_margin; + + internal::updateDistanceLowerBoundFromLeaf( + *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal); + + if (cresult->numContacts() < crequest->num_max_contacts) { + if (distToCollision <= crequest->collision_distance_threshold) { + cresult->addContact(Contact( + tree1, tree2, (int)(root1 - tree1->getRoot()), + static_cast(primitive_id), c1, c2, normal, distance)); + } + } + return crequest->isSatisfied(*cresult); + } + + // Determine which tree to traverse first. + if (bvn2.isLeaf() || + (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) { + for (unsigned int i = 0; i < 8; ++i) { + if (tree1->nodeChildExists(root1, i)) { + const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); + AABB child_bv; + computeChildBV(bv1, i, child_bv); + + if (OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, + tf1, tf2)) + return true; + } + } + } else { + if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, + (unsigned int)bvn2.leftChild(), tf1, tf2)) + return true; + + if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, + (unsigned int)bvn2.rightChild(), tf1, tf2)) + return true; + } + + return false; + } + + /// \return True if the request is satisfied. + template + bool OcTreeHeightFieldIntersectRecurse( + const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, + const HeightField* tree2, unsigned int root2, const Transform3s& tf1, + const Transform3s& tf2, CoalScalar& sqrDistLowerBound) const { + // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The + // code in this if(!root1) did not output anything so the empty OcTree is + // considered free. Should an empty OcTree be considered free ? + + // Empty OcTree is considered free. + if (!root1) return false; + HFNode const& bvn2 = tree2->getBV(root2); + + /// stop when 1) bounding boxes of two objects not overlap; OR + /// 2) at least one of the nodes is free; OR + /// 2) (two uncertain nodes OR one node occupied and one node + /// uncertain) AND cost not required + if (tree1->isNodeFree(root1)) + return false; + else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain())) + return false; + else { + OBB obb1, obb2; + convertBV(bv1, tf1, obb1); + convertBV(bvn2.bv, tf2, obb2); + CoalScalar sqrDistLowerBound_; + if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound_)) { + if (sqrDistLowerBound_ < sqrDistLowerBound) + sqrDistLowerBound = sqrDistLowerBound_; + internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, + sqrDistLowerBound); + return false; + } + } + + // Check if leaf collides. + if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) { + assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. + Box box; + Transform3s box_tf; + constructBox(bv1, tf1, box, box_tf); + if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { + box.computeLocalAABB(); + } + + typedef Convex ConvexTriangle; + ConvexTriangle convex1, convex2; + int convex1_active_faces, convex2_active_faces; + details::buildConvexTriangles(bvn2, *tree2, convex1, convex1_active_faces, + convex2, convex2_active_faces); + if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { + convex1.computeLocalAABB(); + convex2.computeLocalAABB(); + } + + Vec3s c1, c2, normal, normal_top; + CoalScalar distance; + bool hfield_witness_is_on_bin_side; + + bool collision = details::shapeDistance( + solver, *crequest, convex1, convex1_active_faces, convex2, + convex2_active_faces, tf2, box, box_tf, distance, c2, c1, normal, + normal_top, hfield_witness_is_on_bin_side); + + CoalScalar distToCollision = + distance - crequest->security_margin * (normal_top.dot(normal)); + + if (distToCollision <= crequest->collision_distance_threshold) { + sqrDistLowerBound = 0; + if (crequest->num_max_contacts > cresult->numContacts()) { + if (normal_top.isApprox(normal) && + (collision || !hfield_witness_is_on_bin_side)) { + cresult->addContact( + Contact(tree1, tree2, (int)(root1 - tree1->getRoot()), + (int)Contact::NONE, c1, c2, -normal, distance)); + } + } + } else + sqrDistLowerBound = distToCollision * distToCollision; + + // const Vec3s c1 = contact_point - distance * 0.5 * normal; + // const Vec3s c2 = contact_point + distance * 0.5 * normal; + internal::updateDistanceLowerBoundFromLeaf( + *crequest, *cresult, distToCollision, c1, c2, -normal); + + assert(cresult->isCollision() || sqrDistLowerBound > 0); + return crequest->isSatisfied(*cresult); + } + + // Determine which tree to traverse first. + if (bvn2.isLeaf() || + (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) { + for (unsigned int i = 0; i < 8; ++i) { + if (tree1->nodeChildExists(root1, i)) { + const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); + AABB child_bv; + computeChildBV(bv1, i, child_bv); + + if (OcTreeHeightFieldIntersectRecurse(tree1, child, child_bv, tree2, + root2, tf1, tf2, + sqrDistLowerBound)) + return true; + } + } + } else { + if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2, + (unsigned int)bvn2.leftChild(), tf1, + tf2, sqrDistLowerBound)) + return true; + + if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2, + (unsigned int)bvn2.rightChild(), + tf1, tf2, sqrDistLowerBound)) + return true; + } + + return false; + } + + /// \return True if the request is satisfied. + template + bool HeightFieldOcTreeIntersectRecurse( + const HeightField* tree1, unsigned int root1, const OcTree* tree2, + const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3s& tf1, + const Transform3s& tf2, CoalScalar& sqrDistLowerBound) const { + // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The + // code in this if(!root1) did not output anything so the empty OcTree is + // considered free. Should an empty OcTree be considered free ? + + // Empty OcTree is considered free. + if (!root2) return false; + HFNode const& bvn1 = tree1->getBV(root1); + + /// stop when 1) bounding boxes of two objects not overlap; OR + /// 2) at least one of the nodes is free; OR + /// 2) (two uncertain nodes OR one node occupied and one node + /// uncertain) AND cost not required + if (tree2->isNodeFree(root2)) + return false; + else if ((tree2->isNodeUncertain(root2) || tree1->isUncertain())) + return false; + else { + OBB obb1, obb2; + convertBV(bvn1.bv, tf1, obb1); + convertBV(bv2, tf2, obb2); + CoalScalar sqrDistLowerBound_; + if (!obb2.overlap(obb1, *crequest, sqrDistLowerBound_)) { + if (sqrDistLowerBound_ < sqrDistLowerBound) + sqrDistLowerBound = sqrDistLowerBound_; + internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, + sqrDistLowerBound); + return false; + } + } + + // Check if leaf collides. + if (!tree2->nodeHasChildren(root2) && bvn1.isLeaf()) { + assert(tree2->isNodeOccupied(root2)); // it isn't free nor uncertain. + Box box; + Transform3s box_tf; + constructBox(bv2, tf2, box, box_tf); + if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { + box.computeLocalAABB(); + } + + typedef Convex ConvexTriangle; + ConvexTriangle convex1, convex2; + int convex1_active_faces, convex2_active_faces; + details::buildConvexTriangles(bvn1, *tree1, convex1, convex1_active_faces, + convex2, convex2_active_faces); + if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { + convex1.computeLocalAABB(); + convex2.computeLocalAABB(); + } + + Vec3s c1, c2, normal, normal_top; + CoalScalar distance; + bool hfield_witness_is_on_bin_side; + + bool collision = details::shapeDistance( + solver, *crequest, convex1, convex1_active_faces, convex2, + convex2_active_faces, tf1, box, box_tf, distance, c1, c2, normal, + normal_top, hfield_witness_is_on_bin_side); + + CoalScalar distToCollision = + distance - crequest->security_margin * (normal_top.dot(normal)); + + if (distToCollision <= crequest->collision_distance_threshold) { + sqrDistLowerBound = 0; + if (crequest->num_max_contacts > cresult->numContacts()) { + if (normal_top.isApprox(normal) && + (collision || !hfield_witness_is_on_bin_side)) { + cresult->addContact(Contact(tree1, tree2, (int)Contact::NONE, + (int)(root2 - tree2->getRoot()), c1, c2, + normal, distance)); + } + } + } else + sqrDistLowerBound = distToCollision * distToCollision; + + // const Vec3s c1 = contact_point - distance * 0.5 * normal; + // const Vec3s c2 = contact_point + distance * 0.5 * normal; + internal::updateDistanceLowerBoundFromLeaf( + *crequest, *cresult, distToCollision, c1, c2, normal); + + assert(cresult->isCollision() || sqrDistLowerBound > 0); + return crequest->isSatisfied(*cresult); + } + + // Determine which tree to traverse first. + if (bvn1.isLeaf() || + (tree2->nodeHasChildren(root2) && (bv2.size() > bvn1.bv.size()))) { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(bv2, i, child_bv); + + if (HeightFieldOcTreeIntersectRecurse(tree1, root1, tree2, child, + child_bv, tf1, tf2, + sqrDistLowerBound)) + return true; + } + } + } else { + if (HeightFieldOcTreeIntersectRecurse( + tree1, (unsigned int)bvn1.leftChild(), tree2, root2, bv2, tf1, + tf2, sqrDistLowerBound)) + return true; + + if (HeightFieldOcTreeIntersectRecurse( + tree1, (unsigned int)bvn1.rightChild(), tree2, root2, bv2, tf1, + tf2, sqrDistLowerBound)) + return true; + } + + return false; + } + + bool OcTreeDistanceRecurse(const OcTree* tree1, + const OcTree::OcTreeNode* root1, const AABB& bv1, + const OcTree* tree2, + const OcTree::OcTreeNode* root2, const AABB& bv2, + const Transform3s& tf1, + const Transform3s& tf2) const { + if (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) { + if (tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) { + Box box1, box2; + Transform3s box1_tf, box2_tf; + constructBox(bv1, tf1, box1, box1_tf); + constructBox(bv2, tf2, box2, box2_tf); + + if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { + box1.computeLocalAABB(); + box2.computeLocalAABB(); + } + + Vec3s p1, p2, normal; + const CoalScalar distance = internal::ShapeShapeDistance( + &box1, box1_tf, &box2, box2_tf, this->solver, + this->drequest->enable_signed_distance, p1, p2, normal); + + this->dresult->update(distance, tree1, tree2, + (int)(root1 - tree1->getRoot()), + (int)(root2 - tree2->getRoot()), p1, p2, normal); + + return drequest->isSatisfied(*dresult); + } else + return false; + } + + if (!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) + return false; + + if (!tree2->nodeHasChildren(root2) || + (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { + for (unsigned int i = 0; i < 8; ++i) { + if (tree1->nodeChildExists(root1, i)) { + const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); + AABB child_bv; + computeChildBV(bv1, i, child_bv); + + CoalScalar d; + AABB aabb1, aabb2; + convertBV(bv1, tf1, aabb1); + convertBV(bv2, tf2, aabb2); + d = aabb1.distance(aabb2); + + if (d < dresult->min_distance) { + if (OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, + tf1, tf2)) + return true; + } + } + } + } else { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(bv2, i, child_bv); + + CoalScalar d; + AABB aabb1, aabb2; + convertBV(bv1, tf1, aabb1); + convertBV(bv2, tf2, aabb2); + d = aabb1.distance(aabb2); + + if (d < dresult->min_distance) { + if (OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, + tf1, tf2)) + return true; + } + } + } + } + + return false; + } + + bool OcTreeIntersectRecurse(const OcTree* tree1, + const OcTree::OcTreeNode* root1, const AABB& bv1, + const OcTree* tree2, + const OcTree::OcTreeNode* root2, const AABB& bv2, + const Transform3s& tf1, + const Transform3s& tf2) const { + // Empty OcTree is considered free. + if (!root1) return false; + if (!root2) return false; + + /// stop when 1) bounding boxes of two objects not overlap; OR + /// 2) at least one of the nodes is free; OR + /// 2) (two uncertain nodes OR one node occupied and one node + /// uncertain) AND cost not required + if (tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) + return false; + else if ((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2))) + return false; + + bool bothAreLeaves = + (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)); + if (!bothAreLeaves || !crequest->enable_contact) { + OBB obb1, obb2; + convertBV(bv1, tf1, obb1); + convertBV(bv2, tf2, obb2); + CoalScalar sqrDistLowerBound; + if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { + if (cresult->distance_lower_bound > 0 && + sqrDistLowerBound < + cresult->distance_lower_bound * cresult->distance_lower_bound) + cresult->distance_lower_bound = + sqrt(sqrDistLowerBound) - crequest->security_margin; + return false; + } + if (crequest->enable_contact) { // Overlap + if (cresult->numContacts() < crequest->num_max_contacts) + cresult->addContact( + Contact(tree1, tree2, static_cast(root1 - tree1->getRoot()), + static_cast(root2 - tree2->getRoot()))); + return crequest->isSatisfied(*cresult); + } + } + + // Both node are leaves + if (bothAreLeaves) { + assert(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)); + + Box box1, box2; + Transform3s box1_tf, box2_tf; + constructBox(bv1, tf1, box1, box1_tf); + constructBox(bv2, tf2, box2, box2_tf); + + if (this->solver->gjk_initial_guess == + GJKInitialGuess::BoundingVolumeGuess) { + box1.computeLocalAABB(); + box2.computeLocalAABB(); + } + + // When reaching this point, `this->solver` has already been set up + // by the CollisionRequest `this->request`. + // The only thing we need to (and can) pass to `ShapeShapeDistance` is + // whether or not penetration information is should be computed in case of + // collision. + const bool compute_penetration = (this->crequest->enable_contact || + (this->crequest->security_margin < 0)); + Vec3s c1, c2, normal; + CoalScalar distance = internal::ShapeShapeDistance( + &box1, box1_tf, &box2, box2_tf, this->solver, compute_penetration, c1, + c2, normal); + + const CoalScalar distToCollision = + distance - this->crequest->security_margin; + + internal::updateDistanceLowerBoundFromLeaf( + *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal); + + if (this->cresult->numContacts() < this->crequest->num_max_contacts) { + if (distToCollision <= this->crequest->collision_distance_threshold) + this->cresult->addContact( + Contact(tree1, tree2, static_cast(root1 - tree1->getRoot()), + static_cast(root2 - tree2->getRoot()), c1, c2, + normal, distance)); + } + + return crequest->isSatisfied(*cresult); + } + + // Determine which tree to traverse first. + if (!tree2->nodeHasChildren(root2) || + (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { + for (unsigned int i = 0; i < 8; ++i) { + if (tree1->nodeChildExists(root1, i)) { + const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); + AABB child_bv; + computeChildBV(bv1, i, child_bv); + + if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, root2, bv2, + tf1, tf2)) + return true; + } + } + } else { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(bv2, i, child_bv); + + if (OcTreeIntersectRecurse(tree1, root1, bv1, tree2, child, child_bv, + tf1, tf2)) + return true; + } + } + } + + return false; + } +}; + +/// @addtogroup Traversal_For_Collision +/// @{ + +/// @brief Traversal node for octree collision +class COAL_DLLAPI OcTreeCollisionTraversalNode + : public CollisionTraversalNodeBase { + public: + OcTreeCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { + model1 = NULL; + model2 = NULL; + + otsolver = NULL; + } + + bool BVDisjoints(unsigned, unsigned, CoalScalar&) const { return false; } + + void leafCollides(unsigned, unsigned, CoalScalar& sqrDistLowerBound) const { + otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result); + sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound); + sqrDistLowerBound *= sqrDistLowerBound; + } + + const OcTree* model1; + const OcTree* model2; + + Transform3s tf1, tf2; + + const OcTreeSolver* otsolver; +}; + +/// @brief Traversal node for shape-octree collision +template +class COAL_DLLAPI ShapeOcTreeCollisionTraversalNode + : public CollisionTraversalNodeBase { + public: + ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { + model1 = NULL; + model2 = NULL; + + otsolver = NULL; + } + + bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const { + return false; + } + + void leafCollides(unsigned int, unsigned int, + CoalScalar& sqrDistLowerBound) const { + otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result); + sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound); + sqrDistLowerBound *= sqrDistLowerBound; + } + + const S* model1; + const OcTree* model2; + + Transform3s tf1, tf2; + + const OcTreeSolver* otsolver; +}; + +/// @brief Traversal node for octree-shape collision + +template +class COAL_DLLAPI OcTreeShapeCollisionTraversalNode + : public CollisionTraversalNodeBase { + public: + OcTreeShapeCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { + model1 = NULL; + model2 = NULL; + + otsolver = NULL; + } + + bool BVDisjoints(unsigned int, unsigned int, coal::CoalScalar&) const { + return false; + } + + void leafCollides(unsigned int, unsigned int, + CoalScalar& sqrDistLowerBound) const { + otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result); + sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound); + sqrDistLowerBound *= sqrDistLowerBound; + } + + const OcTree* model1; + const S* model2; + + Transform3s tf1, tf2; + + const OcTreeSolver* otsolver; +}; + +/// @brief Traversal node for mesh-octree collision +template +class COAL_DLLAPI MeshOcTreeCollisionTraversalNode + : public CollisionTraversalNodeBase { + public: + MeshOcTreeCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { + model1 = NULL; + model2 = NULL; + + otsolver = NULL; + } + + bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const { + return false; + } + + void leafCollides(unsigned int, unsigned int, + CoalScalar& sqrDistLowerBound) const { + otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result); + sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound); + sqrDistLowerBound *= sqrDistLowerBound; + } + + const BVHModel* model1; + const OcTree* model2; + + Transform3s tf1, tf2; + + const OcTreeSolver* otsolver; +}; + +/// @brief Traversal node for octree-mesh collision +template +class COAL_DLLAPI OcTreeMeshCollisionTraversalNode + : public CollisionTraversalNodeBase { + public: + OcTreeMeshCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { + model1 = NULL; + model2 = NULL; + + otsolver = NULL; + } + + bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const { + return false; + } + + void leafCollides(unsigned int, unsigned int, + CoalScalar& sqrDistLowerBound) const { + otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result); + sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound); + sqrDistLowerBound *= sqrDistLowerBound; + } + + const OcTree* model1; + const BVHModel* model2; + + Transform3s tf1, tf2; + + const OcTreeSolver* otsolver; +}; + +/// @brief Traversal node for octree-height-field collision +template +class COAL_DLLAPI OcTreeHeightFieldCollisionTraversalNode + : public CollisionTraversalNodeBase { + public: + OcTreeHeightFieldCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { + model1 = NULL; + model2 = NULL; + + otsolver = NULL; + } + + bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const { + return false; + } + + void leafCollides(unsigned int, unsigned int, + CoalScalar& sqrDistLowerBound) const { + otsolver->OcTreeHeightFieldIntersect(model1, model2, tf1, tf2, request, + *result, sqrDistLowerBound); + } + + const OcTree* model1; + const HeightField* model2; + + Transform3s tf1, tf2; + + const OcTreeSolver* otsolver; +}; + +/// @brief Traversal node for octree-height-field collision +template +class COAL_DLLAPI HeightFieldOcTreeCollisionTraversalNode + : public CollisionTraversalNodeBase { + public: + HeightFieldOcTreeCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { + model1 = NULL; + model2 = NULL; + + otsolver = NULL; + } + + bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const { + return false; + } + + void leafCollides(unsigned int, unsigned int, + CoalScalar& sqrDistLowerBound) const { + otsolver->HeightFieldOcTreeIntersect(model1, model2, tf1, tf2, request, + *result, sqrDistLowerBound); + } + + const HeightField* model1; + const OcTree* model2; + + Transform3s tf1, tf2; + + const OcTreeSolver* otsolver; +}; + +/// @} + +/// @addtogroup Traversal_For_Distance +/// @{ + +/// @brief Traversal node for octree distance +class COAL_DLLAPI OcTreeDistanceTraversalNode + : public DistanceTraversalNodeBase { + public: + OcTreeDistanceTraversalNode() { + model1 = NULL; + model2 = NULL; + + otsolver = NULL; + } + + CoalScalar BVDistanceLowerBound(unsigned, unsigned) const { return -1; } + + bool BVDistanceLowerBound(unsigned, unsigned, CoalScalar&) const { + return false; + } + + void leafComputeDistance(unsigned, unsigned int) const { + otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result); + } + + const OcTree* model1; + const OcTree* model2; + + const OcTreeSolver* otsolver; +}; + +/// @brief Traversal node for shape-octree distance +template +class COAL_DLLAPI ShapeOcTreeDistanceTraversalNode + : public DistanceTraversalNodeBase { + public: + ShapeOcTreeDistanceTraversalNode() { + model1 = NULL; + model2 = NULL; + + otsolver = NULL; + } + + CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const { + return -1; + } + + void leafComputeDistance(unsigned int, unsigned int) const { + otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result); + } + + const Shape* model1; + const OcTree* model2; + + const OcTreeSolver* otsolver; +}; + +/// @brief Traversal node for octree-shape distance +template +class COAL_DLLAPI OcTreeShapeDistanceTraversalNode + : public DistanceTraversalNodeBase { + public: + OcTreeShapeDistanceTraversalNode() { + model1 = NULL; + model2 = NULL; + + otsolver = NULL; + } + + CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const { + return -1; + } + + void leafComputeDistance(unsigned int, unsigned int) const { + otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result); + } + + const OcTree* model1; + const Shape* model2; + + const OcTreeSolver* otsolver; +}; + +/// @brief Traversal node for mesh-octree distance +template +class COAL_DLLAPI MeshOcTreeDistanceTraversalNode + : public DistanceTraversalNodeBase { + public: + MeshOcTreeDistanceTraversalNode() { + model1 = NULL; + model2 = NULL; + + otsolver = NULL; + } + + CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const { + return -1; + } + + void leafComputeDistance(unsigned int, unsigned int) const { + otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result); + } + + const BVHModel* model1; + const OcTree* model2; + + const OcTreeSolver* otsolver; +}; + +/// @brief Traversal node for octree-mesh distance +template +class COAL_DLLAPI OcTreeMeshDistanceTraversalNode + : public DistanceTraversalNodeBase { + public: + OcTreeMeshDistanceTraversalNode() { + model1 = NULL; + model2 = NULL; + + otsolver = NULL; + } + + CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const { + return -1; + } + + void leafComputeDistance(unsigned int, unsigned int) const { + otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result); + } + + const OcTree* model1; + const BVHModel* model2; + + const OcTreeSolver* otsolver; +}; + +/// @} + +} // namespace coal + +/// @endcond + +#endif diff --git a/include/coal/internal/traversal_node_setup.h b/include/coal/internal/traversal_node_setup.h new file mode 100644 index 000000000..120633edc --- /dev/null +++ b/include/coal/internal/traversal_node_setup.h @@ -0,0 +1,815 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_TRAVERSAL_NODE_SETUP_H +#define COAL_TRAVERSAL_NODE_SETUP_H + +/// @cond INTERNAL + +#include "coal/internal/tools.h" +#include "coal/internal/traversal_node_shapes.h" + +#include "coal/internal/traversal_node_bvhs.h" +#include "coal/internal/traversal_node_bvh_shape.h" + +// #include +#include "coal/internal/traversal_node_hfield_shape.h" + +#ifdef COAL_HAS_OCTOMAP +#include "coal/internal/traversal_node_octree.h" +#endif + +#include "coal/BVH/BVH_utility.h" + +namespace coal { + +#ifdef COAL_HAS_OCTOMAP +/// @brief Initialize traversal node for collision between two octrees, given +/// current object transform +inline bool initialize(OcTreeCollisionTraversalNode& node, const OcTree& model1, + const Transform3s& tf1, const OcTree& model2, + const Transform3s& tf2, const OcTreeSolver* otsolver, + CollisionResult& result) { + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +/// @brief Initialize traversal node for distance between two octrees, given +/// current object transform +inline bool initialize(OcTreeDistanceTraversalNode& node, const OcTree& model1, + const Transform3s& tf1, const OcTree& model2, + const Transform3s& tf2, const OcTreeSolver* otsolver, + const DistanceRequest& request, DistanceResult& result) + +{ + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +/// @brief Initialize traversal node for collision between one shape and one +/// octree, given current object transform +template +bool initialize(ShapeOcTreeCollisionTraversalNode& node, const S& model1, + const Transform3s& tf1, const OcTree& model2, + const Transform3s& tf2, const OcTreeSolver* otsolver, + CollisionResult& result) { + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +/// @brief Initialize traversal node for collision between one octree and one +/// shape, given current object transform +template +bool initialize(OcTreeShapeCollisionTraversalNode& node, + const OcTree& model1, const Transform3s& tf1, const S& model2, + const Transform3s& tf2, const OcTreeSolver* otsolver, + CollisionResult& result) { + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +/// @brief Initialize traversal node for distance between one shape and one +/// octree, given current object transform +template +bool initialize(ShapeOcTreeDistanceTraversalNode& node, const S& model1, + const Transform3s& tf1, const OcTree& model2, + const Transform3s& tf2, const OcTreeSolver* otsolver, + const DistanceRequest& request, DistanceResult& result) { + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +/// @brief Initialize traversal node for distance between one octree and one +/// shape, given current object transform +template +bool initialize(OcTreeShapeDistanceTraversalNode& node, const OcTree& model1, + const Transform3s& tf1, const S& model2, const Transform3s& tf2, + const OcTreeSolver* otsolver, const DistanceRequest& request, + DistanceResult& result) { + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +/// @brief Initialize traversal node for collision between one mesh and one +/// octree, given current object transform +template +bool initialize(MeshOcTreeCollisionTraversalNode& node, + const BVHModel& model1, const Transform3s& tf1, + const OcTree& model2, const Transform3s& tf2, + const OcTreeSolver* otsolver, CollisionResult& result) { + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +/// @brief Initialize traversal node for collision between one octree and one +/// mesh, given current object transform +template +bool initialize(OcTreeMeshCollisionTraversalNode& node, + const OcTree& model1, const Transform3s& tf1, + const BVHModel& model2, const Transform3s& tf2, + const OcTreeSolver* otsolver, CollisionResult& result) { + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +/// @brief Initialize traversal node for collision between one octree and one +/// height field, given current object transform +template +bool initialize(OcTreeHeightFieldCollisionTraversalNode& node, + const OcTree& model1, const Transform3s& tf1, + const HeightField& model2, const Transform3s& tf2, + const OcTreeSolver* otsolver, CollisionResult& result) { + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +/// @brief Initialize traversal node for collision between one height field and +/// one octree, given current object transform +template +bool initialize(HeightFieldOcTreeCollisionTraversalNode& node, + const HeightField& model1, const Transform3s& tf1, + const OcTree& model2, const Transform3s& tf2, + const OcTreeSolver* otsolver, CollisionResult& result) { + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +/// @brief Initialize traversal node for distance between one mesh and one +/// octree, given current object transform +template +bool initialize(MeshOcTreeDistanceTraversalNode& node, + const BVHModel& model1, const Transform3s& tf1, + const OcTree& model2, const Transform3s& tf2, + const OcTreeSolver* otsolver, const DistanceRequest& request, + DistanceResult& result) { + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +/// @brief Initialize traversal node for collision between one octree and one +/// mesh, given current object transform +template +bool initialize(OcTreeMeshDistanceTraversalNode& node, const OcTree& model1, + const Transform3s& tf1, const BVHModel& model2, + const Transform3s& tf2, const OcTreeSolver* otsolver, + const DistanceRequest& request, DistanceResult& result) { + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +#endif + +/// @brief Initialize traversal node for collision between two geometric shapes, +/// given current object transform +template +bool initialize(ShapeCollisionTraversalNode& node, const S1& shape1, + const Transform3s& tf1, const S2& shape2, + const Transform3s& tf2, const GJKSolver* nsolver, + CollisionResult& result) { + node.model1 = &shape1; + node.tf1 = tf1; + node.model2 = &shape2; + node.tf2 = tf2; + node.nsolver = nsolver; + + node.result = &result; + + return true; +} + +/// @brief Initialize traversal node for collision between one mesh and one +/// shape, given current object transform +template +bool initialize(MeshShapeCollisionTraversalNode& node, + BVHModel& model1, Transform3s& tf1, const S& model2, + const Transform3s& tf2, const GJKSolver* nsolver, + CollisionResult& result, bool use_refit = false, + bool refit_bottomup = false) { + if (model1.getModelType() != BVH_MODEL_TRIANGLES) + COAL_THROW_PRETTY( + "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + + if (!tf1.isIdentity() && + model1.vertices.get()) // TODO(jcarpent): vectorized version + { + std::vector vertices_transformed(model1.num_vertices); + const std::vector& model1_vertices_ = *(model1.vertices); + for (unsigned int i = 0; i < model1.num_vertices; ++i) { + const Vec3s& p = model1_vertices_[i]; + Vec3s new_v = tf1.transform(p); + vertices_transformed[i] = new_v; + } + + model1.beginReplaceModel(); + model1.replaceSubModel(vertices_transformed); + model1.endReplaceModel(use_refit, refit_bottomup); + + tf1.setIdentity(); + } + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + computeBV(model2, tf2, node.model2_bv); + + node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL; + node.tri_indices = + model1.tri_indices.get() ? model1.tri_indices->data() : NULL; + + node.result = &result; + + return true; +} + +/// @brief Initialize traversal node for collision between one mesh and one +/// shape +template +bool initialize(MeshShapeCollisionTraversalNode& node, + const BVHModel& model1, const Transform3s& tf1, + const S& model2, const Transform3s& tf2, + const GJKSolver* nsolver, CollisionResult& result) { + if (model1.getModelType() != BVH_MODEL_TRIANGLES) + COAL_THROW_PRETTY( + "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + computeBV(model2, tf2, node.model2_bv); + + node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL; + node.tri_indices = + model1.tri_indices.get() ? model1.tri_indices->data() : NULL; + + node.result = &result; + + return true; +} + +/// @brief Initialize traversal node for collision between one mesh and one +/// shape, given current object transform +template +bool initialize(HeightFieldShapeCollisionTraversalNode& node, + HeightField& model1, Transform3s& tf1, const S& model2, + const Transform3s& tf2, const GJKSolver* nsolver, + CollisionResult& result, bool use_refit = false, + bool refit_bottomup = false); + +/// @brief Initialize traversal node for collision between one mesh and one +/// shape +template +bool initialize(HeightFieldShapeCollisionTraversalNode& node, + const HeightField& model1, const Transform3s& tf1, + const S& model2, const Transform3s& tf2, + const GJKSolver* nsolver, CollisionResult& result) { + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + computeBV(model2, tf2, node.model2_bv); + + node.result = &result; + + return true; +} + +/// @cond IGNORE +namespace details { +template class OrientedNode> +static inline bool setupShapeMeshCollisionOrientedNode( + OrientedNode& node, const S& model1, const Transform3s& tf1, + const BVHModel& model2, const Transform3s& tf2, + const GJKSolver* nsolver, CollisionResult& result) { + if (model2.getModelType() != BVH_MODEL_TRIANGLES) + COAL_THROW_PRETTY( + "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + computeBV(model1, tf1, node.model1_bv); + + node.vertices = model2.vertices.get() ? model2.vertices->data() : NULL; + node.tri_indices = + model2.tri_indices.get() ? model2.tri_indices->data() : NULL; + + node.result = &result; + + return true; +} +} // namespace details +/// @endcond + +/// @brief Initialize traversal node for collision between two meshes, given the +/// current transforms +template +bool initialize( + MeshCollisionTraversalNode& node, + BVHModel& model1, Transform3s& tf1, BVHModel& model2, + Transform3s& tf2, CollisionResult& result, bool use_refit = false, + bool refit_bottomup = false) { + if (model1.getModelType() != BVH_MODEL_TRIANGLES) + COAL_THROW_PRETTY( + "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + if (model2.getModelType() != BVH_MODEL_TRIANGLES) + COAL_THROW_PRETTY( + "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + + if (!tf1.isIdentity() && model1.vertices.get()) { + std::vector vertices_transformed1(model1.num_vertices); + const std::vector& model1_vertices_ = *(model1.vertices); + for (unsigned int i = 0; i < model1.num_vertices; ++i) { + const Vec3s& p = model1_vertices_[i]; + Vec3s new_v = tf1.transform(p); + vertices_transformed1[i] = new_v; + } + + model1.beginReplaceModel(); + model1.replaceSubModel(vertices_transformed1); + model1.endReplaceModel(use_refit, refit_bottomup); + + tf1.setIdentity(); + } + + if (!tf2.isIdentity() && model2.vertices.get()) { + std::vector vertices_transformed2(model2.num_vertices); + const std::vector& model2_vertices_ = *(model2.vertices); + for (unsigned int i = 0; i < model2.num_vertices; ++i) { + const Vec3s& p = model2_vertices_[i]; + Vec3s new_v = tf2.transform(p); + vertices_transformed2[i] = new_v; + } + + model2.beginReplaceModel(); + model2.replaceSubModel(vertices_transformed2); + model2.endReplaceModel(use_refit, refit_bottomup); + + tf2.setIdentity(); + } + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + + node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL; + node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL; + + node.tri_indices1 = + model1.tri_indices.get() ? model1.tri_indices->data() : NULL; + node.tri_indices2 = + model2.tri_indices.get() ? model2.tri_indices->data() : NULL; + + node.result = &result; + + return true; +} + +template +bool initialize(MeshCollisionTraversalNode& node, + const BVHModel& model1, const Transform3s& tf1, + const BVHModel& model2, const Transform3s& tf2, + CollisionResult& result) { + if (model1.getModelType() != BVH_MODEL_TRIANGLES) + COAL_THROW_PRETTY( + "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + if (model2.getModelType() != BVH_MODEL_TRIANGLES) + COAL_THROW_PRETTY( + "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + + node.vertices1 = model1.vertices ? model1.vertices->data() : NULL; + node.vertices2 = model2.vertices ? model2.vertices->data() : NULL; + + node.tri_indices1 = + model1.tri_indices.get() ? model1.tri_indices->data() : NULL; + node.tri_indices2 = + model2.tri_indices.get() ? model2.tri_indices->data() : NULL; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + + node.result = &result; + + node.RT.R.noalias() = tf1.getRotation().transpose() * tf2.getRotation(); + node.RT.T.noalias() = tf1.getRotation().transpose() * + (tf2.getTranslation() - tf1.getTranslation()); + + return true; +} + +/// @brief Initialize traversal node for distance between two geometric shapes +template +bool initialize(ShapeDistanceTraversalNode& node, const S1& shape1, + const Transform3s& tf1, const S2& shape2, + const Transform3s& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, DistanceResult& result) { + node.request = request; + node.result = &result; + + node.model1 = &shape1; + node.tf1 = tf1; + node.model2 = &shape2; + node.tf2 = tf2; + node.nsolver = nsolver; + + return true; +} + +/// @brief Initialize traversal node for distance computation between two +/// meshes, given the current transforms +template +bool initialize( + MeshDistanceTraversalNode& node, + BVHModel& model1, Transform3s& tf1, BVHModel& model2, + Transform3s& tf2, const DistanceRequest& request, DistanceResult& result, + bool use_refit = false, bool refit_bottomup = false) { + if (model1.getModelType() != BVH_MODEL_TRIANGLES) + COAL_THROW_PRETTY( + "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + if (model2.getModelType() != BVH_MODEL_TRIANGLES) + COAL_THROW_PRETTY( + "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + + if (!tf1.isIdentity() && model1.vertices.get()) { + std::vector vertices_transformed1(model1.num_vertices); + const std::vector& model1_vertices_ = *(model1.vertices); + for (unsigned int i = 0; i < model1.num_vertices; ++i) { + const Vec3s& p = model1_vertices_[i]; + Vec3s new_v = tf1.transform(p); + vertices_transformed1[i] = new_v; + } + + model1.beginReplaceModel(); + model1.replaceSubModel(vertices_transformed1); + model1.endReplaceModel(use_refit, refit_bottomup); + + tf1.setIdentity(); + } + + if (!tf2.isIdentity() && model2.vertices.get()) { + std::vector vertices_transformed2(model2.num_vertices); + const std::vector& model2_vertices_ = *(model2.vertices); + for (unsigned int i = 0; i < model2.num_vertices; ++i) { + const Vec3s& p = model2_vertices_[i]; + Vec3s new_v = tf2.transform(p); + vertices_transformed2[i] = new_v; + } + + model2.beginReplaceModel(); + model2.replaceSubModel(vertices_transformed2); + model2.endReplaceModel(use_refit, refit_bottomup); + + tf2.setIdentity(); + } + + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + + node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL; + node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL; + + node.tri_indices1 = + model1.tri_indices.get() ? model1.tri_indices->data() : NULL; + node.tri_indices2 = + model2.tri_indices.get() ? model2.tri_indices->data() : NULL; + + return true; +} + +/// @brief Initialize traversal node for distance computation between two meshes +template +bool initialize(MeshDistanceTraversalNode& node, + const BVHModel& model1, const Transform3s& tf1, + const BVHModel& model2, const Transform3s& tf2, + const DistanceRequest& request, DistanceResult& result) { + if (model1.getModelType() != BVH_MODEL_TRIANGLES) + COAL_THROW_PRETTY( + "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + if (model2.getModelType() != BVH_MODEL_TRIANGLES) + COAL_THROW_PRETTY( + "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + + node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL; + node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL; + + node.tri_indices1 = + model1.tri_indices.get() ? model1.tri_indices->data() : NULL; + node.tri_indices2 = + model2.tri_indices.get() ? model2.tri_indices->data() : NULL; + + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED + + relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), + tf2.getTranslation(), node.RT.R, node.RT.T); + + COAL_COMPILER_DIAGNOSTIC_POP + + return true; +} + +/// @brief Initialize traversal node for distance computation between one mesh +/// and one shape, given the current transforms +template +bool initialize(MeshShapeDistanceTraversalNode& node, + BVHModel& model1, Transform3s& tf1, const S& model2, + const Transform3s& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, DistanceResult& result, + bool use_refit = false, bool refit_bottomup = false) { + if (model1.getModelType() != BVH_MODEL_TRIANGLES) + COAL_THROW_PRETTY( + "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + + if (!tf1.isIdentity() && model1.vertices.get()) { + const std::vector& model1_vertices_ = *(model1.vertices); + std::vector vertices_transformed1(model1.num_vertices); + for (unsigned int i = 0; i < model1.num_vertices; ++i) { + const Vec3s& p = model1_vertices_[i]; + Vec3s new_v = tf1.transform(p); + vertices_transformed1[i] = new_v; + } + + model1.beginReplaceModel(); + model1.replaceSubModel(vertices_transformed1); + model1.endReplaceModel(use_refit, refit_bottomup); + + tf1.setIdentity(); + } + + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL; + node.tri_indices = + model1.tri_indices.get() ? model1.tri_indices->data() : NULL; + + computeBV(model2, tf2, node.model2_bv); + + return true; +} + +/// @cond IGNORE +namespace details { + +template class OrientedNode> +static inline bool setupMeshShapeDistanceOrientedNode( + OrientedNode& node, const BVHModel& model1, const Transform3s& tf1, + const S& model2, const Transform3s& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, DistanceResult& result) { + if (model1.getModelType() != BVH_MODEL_TRIANGLES) + COAL_THROW_PRETTY( + "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + computeBV(model2, tf2, node.model2_bv); + + node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL; + node.tri_indices = + model1.tri_indices.get() ? model1.tri_indices->data() : NULL; + + return true; +} +} // namespace details +/// @endcond + +/// @brief Initialize traversal node for distance computation between one mesh +/// and one shape, specialized for RSS type +template +bool initialize(MeshShapeDistanceTraversalNodeRSS& node, + const BVHModel& model1, const Transform3s& tf1, + const S& model2, const Transform3s& tf2, + const GJKSolver* nsolver, const DistanceRequest& request, + DistanceResult& result) { + return details::setupMeshShapeDistanceOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); +} + +/// @brief Initialize traversal node for distance computation between one mesh +/// and one shape, specialized for kIOS type +template +bool initialize(MeshShapeDistanceTraversalNodekIOS& node, + const BVHModel& model1, const Transform3s& tf1, + const S& model2, const Transform3s& tf2, + const GJKSolver* nsolver, const DistanceRequest& request, + DistanceResult& result) { + return details::setupMeshShapeDistanceOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); +} + +/// @brief Initialize traversal node for distance computation between one mesh +/// and one shape, specialized for OBBRSS type +template +bool initialize(MeshShapeDistanceTraversalNodeOBBRSS& node, + const BVHModel& model1, const Transform3s& tf1, + const S& model2, const Transform3s& tf2, + const GJKSolver* nsolver, const DistanceRequest& request, + DistanceResult& result) { + return details::setupMeshShapeDistanceOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); +} + +} // namespace coal + +/// @endcond + +#endif diff --git a/include/coal/internal/traversal_node_shapes.h b/include/coal/internal/traversal_node_shapes.h new file mode 100644 index 000000000..a10d8b715 --- /dev/null +++ b/include/coal/internal/traversal_node_shapes.h @@ -0,0 +1,124 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_TRAVERSAL_NODE_SHAPES_H +#define COAL_TRAVERSAL_NODE_SHAPES_H + +/// @cond INTERNAL + +#include "coal/collision_data.h" +#include "coal/BV/BV.h" +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/internal/traversal_node_base.h" +#include "coal/internal/shape_shape_func.h" + +namespace coal { + +/// @addtogroup Traversal_For_Collision +/// @{ + +/// @brief Traversal node for collision between two shapes +template +class COAL_DLLAPI ShapeCollisionTraversalNode + : public CollisionTraversalNodeBase { + public: + ShapeCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { + model1 = NULL; + model2 = NULL; + + nsolver = NULL; + } + + /// @brief BV culling test in one BVTT node + bool BVDisjoints(int, int, CoalScalar&) const { + COAL_THROW_PRETTY("Not implemented", std::runtime_error); + } + + /// @brief Intersection testing between leaves (two shapes) + void leafCollides(int, int, CoalScalar&) const { + ShapeShapeCollide(this->model1, this->tf1, this->model2, this->tf2, + this->nsolver, this->request, *(this->result)); + } + + const S1* model1; + const S2* model2; + + const GJKSolver* nsolver; +}; + +/// @} + +/// @addtogroup Traversal_For_Distance +/// @{ + +/// @brief Traversal node for distance between two shapes +template +class COAL_DLLAPI ShapeDistanceTraversalNode + : public DistanceTraversalNodeBase { + public: + ShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { + model1 = NULL; + model2 = NULL; + + nsolver = NULL; + } + + /// @brief BV culling test in one BVTT node + CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const { + return -1; // should not be used + } + + /// @brief Distance testing between leaves (two shapes) + void leafComputeDistance(unsigned int, unsigned int) const { + ShapeShapeDistance(this->model1, this->tf1, this->model2, this->tf2, + this->nsolver, this->request, *(this->result)); + } + + const S1* model1; + const S2* model2; + + const GJKSolver* nsolver; +}; + +/// @} + +} // namespace coal + +/// @endcond + +#endif diff --git a/include/coal/internal/traversal_recurse.h b/include/coal/internal/traversal_recurse.h new file mode 100644 index 000000000..c0626b514 --- /dev/null +++ b/include/coal/internal/traversal_recurse.h @@ -0,0 +1,81 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_TRAVERSAL_RECURSE_H +#define COAL_TRAVERSAL_RECURSE_H + +/// @cond INTERNAL + +#include "coal/BVH/BVH_front.h" +#include "coal/internal/traversal_node_base.h" +#include "coal/internal/traversal_node_bvhs.h" +#include + +namespace coal { + +/// Recurse function for collision +/// @param node collision node, +/// @param b1, b2 ids of bounding volume nodes for object 1 and object 2 +/// @retval sqrDistLowerBound squared lower bound on distance between objects. +void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1, + unsigned int b2, BVHFrontList* front_list, + CoalScalar& sqrDistLowerBound); + +void collisionNonRecurse(CollisionTraversalNodeBase* node, + BVHFrontList* front_list, + CoalScalar& sqrDistLowerBound); + +/// @brief Recurse function for distance +void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1, + unsigned int b2, BVHFrontList* front_list); + +/// @brief Recurse function for distance, using queue acceleration +void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1, + unsigned int b2, BVHFrontList* front_list, + unsigned int qsize); + +/// @brief Recurse function for front list propagation +void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, + const CollisionRequest& request, + CollisionResult& result, + BVHFrontList* front_list); + +} // namespace coal + +/// @endcond + +#endif diff --git a/include/coal/logging.h b/include/coal/logging.h new file mode 100644 index 000000000..235b475c5 --- /dev/null +++ b/include/coal/logging.h @@ -0,0 +1,54 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/// This file defines basic logging macros for Coal, based on Boost.Log. +/// To enable logging, define the preprocessor macro `COAL_ENABLE_LOGGING`. + +#ifndef COAL_LOGGING_H +#define COAL_LOGGING_H + +#ifdef COAL_ENABLE_LOGGING +#include +#define COAL_LOG_INFO(message) BOOST_LOG_TRIVIAL(info) << message +#define COAL_LOG_DEBUG(message) BOOST_LOG_TRIVIAL(debug) << message +#define COAL_LOG_WARNING(message) BOOST_LOG_TRIVIAL(warning) << message +#define COAL_LOG_ERROR(message) BOOST_LOG_TRIVIAL(error) << message +#else +#define COAL_LOG_INFO(message) +#define COAL_LOG_DEBUG(message) +#define COAL_LOG_WARNING(message) +#define COAL_LOG_ERROR(message) +#endif + +#endif // COAL_LOGGING_H diff --git a/include/coal/math/matrix_3f.h b/include/coal/math/matrix_3f.h new file mode 100644 index 000000000..70f555a02 --- /dev/null +++ b/include/coal/math/matrix_3f.h @@ -0,0 +1,46 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_MATRIX_3F_H +#define COAL_MATRIX_3F_H + +#warning "This file is deprecated. Include instead." + +// List of equivalent includes. +#include "coal/data_types.h" + +#endif diff --git a/include/coal/math/transform.h b/include/coal/math/transform.h new file mode 100644 index 000000000..7d712fadc --- /dev/null +++ b/include/coal/math/transform.h @@ -0,0 +1,272 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_TRANSFORM_H +#define COAL_TRANSFORM_H + +#include "coal/fwd.hh" +#include "coal/data_types.h" + +namespace coal { + +COAL_DEPRECATED typedef Eigen::Quaternion Quaternion3f; +typedef Eigen::Quaternion Quatf; + +static inline std::ostream& operator<<(std::ostream& o, const Quatf& q) { + o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")"; + return o; +} + +/// @brief Simple transform class used locally by InterpMotion +class COAL_DLLAPI Transform3s { + /// @brief Matrix cache + Matrix3s R; + + /// @brief Translation vector + Vec3s T; + + public: + /// @brief Default transform is no movement + Transform3s() { + setIdentity(); // set matrix_set true + } + + static Transform3s Identity() { return Transform3s(); } + + /// @brief Construct transform from rotation and translation + template + Transform3s(const Eigen::MatrixBase& R_, + const Eigen::MatrixBase& T_) + : R(R_), T(T_) {} + + /// @brief Construct transform from rotation and translation + template + Transform3s(const Quatf& q_, const Eigen::MatrixBase& T_) + : R(q_.toRotationMatrix()), T(T_) {} + + /// @brief Construct transform from rotation + Transform3s(const Matrix3s& R_) : R(R_), T(Vec3s::Zero()) {} + + /// @brief Construct transform from rotation + Transform3s(const Quatf& q_) : R(q_), T(Vec3s::Zero()) {} + + /// @brief Construct transform from translation + Transform3s(const Vec3s& T_) : R(Matrix3s::Identity()), T(T_) {} + + /// @brief Construct transform from other transform + Transform3s(const Transform3s& tf) : R(tf.R), T(tf.T) {} + + /// @brief operator = + Transform3s& operator=(const Transform3s& tf) { + R = tf.R; + T = tf.T; + return *this; + } + + /// @brief get translation + inline const Vec3s& getTranslation() const { return T; } + + /// @brief get translation + inline const Vec3s& translation() const { return T; } + + /// @brief get translation + inline Vec3s& translation() { return T; } + + /// @brief get rotation + inline const Matrix3s& getRotation() const { return R; } + + /// @brief get rotation + inline const Matrix3s& rotation() const { return R; } + + /// @brief get rotation + inline Matrix3s& rotation() { return R; } + + /// @brief get quaternion + inline Quatf getQuatRotation() const { return Quatf(R); } + + /// @brief set transform from rotation and translation + template + inline void setTransform(const Eigen::MatrixBase& R_, + const Eigen::MatrixBase& T_) { + R.noalias() = R_; + T.noalias() = T_; + } + + /// @brief set transform from rotation and translation + inline void setTransform(const Quatf& q_, const Vec3s& T_) { + R = q_.toRotationMatrix(); + T = T_; + } + + /// @brief set transform from rotation + template + inline void setRotation(const Eigen::MatrixBase& R_) { + R.noalias() = R_; + } + + /// @brief set transform from translation + template + inline void setTranslation(const Eigen::MatrixBase& T_) { + T.noalias() = T_; + } + + /// @brief set transform from rotation + inline void setQuatRotation(const Quatf& q_) { R = q_.toRotationMatrix(); } + + /// @brief transform a given vector by the transform + template + inline Vec3s transform(const Eigen::MatrixBase& v) const { + return R * v + T; + } + + /// @brief transform a given vector by the inverse of the transform + template + inline Vec3s inverseTransform(const Eigen::MatrixBase& v) const { + return R.transpose() * (v - T); + } + + /// @brief inverse transform + inline Transform3s& inverseInPlace() { + R.transposeInPlace(); + T = -R * T; + return *this; + } + + /// @brief inverse transform + inline Transform3s inverse() { + return Transform3s(R.transpose(), -R.transpose() * T); + } + + /// @brief inverse the transform and multiply with another + inline Transform3s inverseTimes(const Transform3s& other) const { + return Transform3s(R.transpose() * other.R, R.transpose() * (other.T - T)); + } + + /// @brief multiply with another transform + inline const Transform3s& operator*=(const Transform3s& other) { + T += R * other.T; + R *= other.R; + return *this; + } + + /// @brief multiply with another transform + inline Transform3s operator*(const Transform3s& other) const { + return Transform3s(R * other.R, R * other.T + T); + } + + /// @brief check whether the transform is identity + inline bool isIdentity( + const CoalScalar& prec = + Eigen::NumTraits::dummy_precision()) const { + return R.isIdentity(prec) && T.isZero(prec); + } + + /// @brief set the transform to be identity transform + inline void setIdentity() { + R.setIdentity(); + T.setZero(); + } + + /// @brief return a random transform + static Transform3s Random() { + Transform3s tf = Transform3s(); + tf.setRandom(); + return tf; + } + + /// @brief set the transform to a random transform + inline void setRandom(); + + bool operator==(const Transform3s& other) const { + return (R == other.getRotation()) && (T == other.getTranslation()); + } + + bool operator!=(const Transform3s& other) const { return !(*this == other); } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +template +inline Quatf fromAxisAngle(const Eigen::MatrixBase& axis, + CoalScalar angle) { + return Quatf(Eigen::AngleAxis(angle, axis)); +} + +/// @brief Uniformly random quaternion sphere. +/// Code taken from Pinocchio (https://github.com/stack-of-tasks/pinocchio). +inline Quatf uniformRandomQuaternion() { + // Rotational part + const CoalScalar u1 = (CoalScalar)rand() / RAND_MAX; + const CoalScalar u2 = (CoalScalar)rand() / RAND_MAX; + const CoalScalar u3 = (CoalScalar)rand() / RAND_MAX; + + const CoalScalar mult1 = std::sqrt(CoalScalar(1.0) - u1); + const CoalScalar mult2 = std::sqrt(u1); + + static const CoalScalar PI_value = static_cast(EIGEN_PI); + CoalScalar s2 = std::sin(2 * PI_value * u2); + CoalScalar c2 = std::cos(2 * PI_value * u2); + CoalScalar s3 = std::sin(2 * PI_value * u3); + CoalScalar c3 = std::cos(2 * PI_value * u3); + + Quatf q; + q.w() = mult1 * s2; + q.x() = mult1 * c2; + q.y() = mult2 * s3; + q.z() = mult2 * c3; + return q; +} + +inline void Transform3s::setRandom() { + const Quatf q = uniformRandomQuaternion(); + this->rotation() = q.matrix(); + this->translation().setRandom(); +} + +/// @brief Construct othonormal basis from vector. +/// The z-axis is the normalized input vector. +inline Matrix3s constructOrthonormalBasisFromVector(const Vec3s& vec) { + Matrix3s basis = Matrix3s::Zero(); + basis.col(2) = vec.normalized(); + basis.col(1) = -vec.unitOrthogonal(); + basis.col(0) = basis.col(1).cross(vec); + return basis; +} + +} // namespace coal + +#endif diff --git a/include/coal/math/types.h b/include/coal/math/types.h new file mode 100644 index 000000000..4b9b5a91f --- /dev/null +++ b/include/coal/math/types.h @@ -0,0 +1,46 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Joseph Mirabel */ + +#ifndef COAL_MATH_TYPES_H +#define COAL_MATH_TYPES_H + +#warning "This file is deprecated. Include instead." + +// List of equivalent includes. +#include "coal/data_types.h" + +#endif diff --git a/include/coal/math/vec_3f.h b/include/coal/math/vec_3f.h new file mode 100644 index 000000000..5b79b2191 --- /dev/null +++ b/include/coal/math/vec_3f.h @@ -0,0 +1,46 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_VEC_3F_H +#define COAL_VEC_3F_H + +#warning "This file is deprecated. Include instead." + +// List of equivalent includes. +#include "coal/data_types.h" + +#endif diff --git a/include/coal/mesh_loader/assimp.h b/include/coal/mesh_loader/assimp.h new file mode 100644 index 000000000..0133f4569 --- /dev/null +++ b/include/coal/mesh_loader/assimp.h @@ -0,0 +1,127 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2016-2019, CNRS - LAAS + * Copyright (c) 2019, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef COAL_MESH_LOADER_ASSIMP_H +#define COAL_MESH_LOADER_ASSIMP_H + +#include "coal/fwd.hh" +#include "coal/config.hh" +#include "coal/BV/OBBRSS.h" +#include "coal/BVH/BVH_model.h" + +struct aiScene; +namespace Assimp { +class Importer; +} + +namespace coal { + +namespace internal { + +struct COAL_DLLAPI TriangleAndVertices { + std::vector vertices_; + std::vector triangles_; +}; + +struct COAL_DLLAPI Loader { + Loader(); + ~Loader(); + + void load(const std::string& resource_path); + + Assimp::Importer* importer; + aiScene const* scene; +}; + +/** + * @brief Recursive procedure for building a mesh + * + * @param[in] scale Scale to apply when reading the ressource + * @param[in] scene Pointer to the assimp scene + * @param[in] vertices_offset Current number of vertices in the model + * @param tv Triangles and Vertices of the mesh submodels + */ +COAL_DLLAPI void buildMesh(const coal::Vec3s& scale, const aiScene* scene, + unsigned vertices_offset, TriangleAndVertices& tv); + +/** + * @brief Convert an assimp scene to a mesh + * + * @param[in] scale Scale to apply when reading the ressource + * @param[in] scene Pointer to the assimp scene + * @param[out] mesh The mesh that must be built + */ +template +inline void meshFromAssimpScene( + const coal::Vec3s& scale, const aiScene* scene, + const shared_ptr >& mesh) { + TriangleAndVertices tv; + + int res = mesh->beginModel(); + + if (res != coal::BVH_OK) { + COAL_THROW_PRETTY("fcl BVHReturnCode = " << res, std::runtime_error); + } + + buildMesh(scale, scene, (unsigned)mesh->num_vertices, tv); + mesh->addSubModel(tv.vertices_, tv.triangles_); + + mesh->endModel(); +} + +} // namespace internal + +/** + * @brief Read a mesh file and convert it to a polyhedral mesh + * + * @param[in] resource_path Path to the ressource mesh file to be read + * @param[in] scale Scale to apply when reading the ressource + * @param[out] polyhedron The resulted polyhedron + */ +template +inline void loadPolyhedronFromResource( + const std::string& resource_path, const coal::Vec3s& scale, + const shared_ptr >& polyhedron) { + internal::Loader scene; + scene.load(resource_path); + + internal::meshFromAssimpScene(scale, scene.scene, polyhedron); +} + +} // namespace coal + +#endif // COAL_MESH_LOADER_ASSIMP_H diff --git a/include/coal/mesh_loader/loader.h b/include/coal/mesh_loader/loader.h new file mode 100644 index 000000000..18b2255b8 --- /dev/null +++ b/include/coal/mesh_loader/loader.h @@ -0,0 +1,102 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2016, CNRS - LAAS + * Copyright (c) 2020, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef COAL_MESH_LOADER_LOADER_H +#define COAL_MESH_LOADER_LOADER_H + +#include "coal/fwd.hh" +#include "coal/config.hh" +#include "coal/data_types.h" +#include "coal/collision_object.h" + +#include +#include + +namespace coal { +/// Base class for building polyhedron from files. +/// This class builds a new object for each file. +class COAL_DLLAPI MeshLoader { + public: + virtual ~MeshLoader() {} + + virtual BVHModelPtr_t load(const std::string& filename, + const Vec3s& scale = Vec3s::Ones()); + + /// Create an OcTree from a file in binary octomap format. + /// \todo add OctreePtr_t + virtual CollisionGeometryPtr_t loadOctree(const std::string& filename); + + MeshLoader(const NODE_TYPE& bvType = BV_OBBRSS) : bvType_(bvType) {} + + private: + const NODE_TYPE bvType_; +}; + +/// Class for building polyhedron from files with cache mechanism. +/// This class builds a new object for each different file. +/// If method CachedMeshLoader::load is called twice with the same arguments, +/// the second call returns the result of the first call. +class COAL_DLLAPI CachedMeshLoader : public MeshLoader { + public: + virtual ~CachedMeshLoader() {} + + CachedMeshLoader(const NODE_TYPE& bvType = BV_OBBRSS) : MeshLoader(bvType) {} + + virtual BVHModelPtr_t load(const std::string& filename, const Vec3s& scale); + + struct COAL_DLLAPI Key { + std::string filename; + Vec3s scale; + + Key(const std::string& f, const Vec3s& s) : filename(f), scale(s) {} + + bool operator<(const CachedMeshLoader::Key& b) const; + }; + struct COAL_DLLAPI Value { + BVHModelPtr_t model; + std::time_t mtime; + }; + typedef std::map Cache_t; + + const Cache_t& cache() const { return cache_; } + + private: + Cache_t cache_; +}; +} // namespace coal + +#endif // COAL_MESH_LOADER_LOADER_H diff --git a/include/coal/narrowphase/gjk.h b/include/coal/narrowphase/gjk.h new file mode 100644 index 000000000..ea8ed8f03 --- /dev/null +++ b/include/coal/narrowphase/gjk.h @@ -0,0 +1,454 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2021-2024, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_GJK_H +#define COAL_GJK_H + +#include + +#include "coal/narrowphase/minkowski_difference.h" + +namespace coal { + +namespace details { + +/// @brief class for GJK algorithm +/// +/// @note The computations are performed in the frame of the first shape. +struct COAL_DLLAPI GJK { + struct COAL_DLLAPI SimplexV { + /// @brief support vector for shape 0 and 1. + Vec3s w0, w1; + /// @brief support vector (i.e., the furthest point on the shape along the + /// support direction) + Vec3s w; + }; + + typedef unsigned char vertex_id_t; + + /// @brief A simplex is a set of up to 4 vertices. + /// Its rank is the number of vertices it contains. + /// @note This data structure does **not** own the vertices it refers to. + /// To be efficient, the constructor of `GJK` creates storage for 4 vertices. + /// Since GJK does not need any more storage, it reuses these vertices + /// throughout the algorithm by using multiple instance of this `Simplex` + /// class. + struct COAL_DLLAPI Simplex { + /// @brief simplex vertex + SimplexV* vertex[4]; + /// @brief size of simplex (number of vertices) + vertex_id_t rank; + + Simplex() {} + + void reset() { + rank = 0; + for (size_t i = 0; i < 4; ++i) vertex[i] = nullptr; + } + }; + + /// @brief Status of the GJK algorithm: + /// DidNotRun: GJK has not been run. + /// Failed: GJK did not converge (it exceeded the maximum number of + /// iterations). + /// NoCollisionEarlyStopped: GJK found a separating hyperplane and exited + /// before converting. The shapes are not in collision. + /// NoCollision: GJK converged and the shapes are not in collision. + /// Collision: GJK converged and the shapes are in collision. + /// Failed: GJK did not converge. + enum Status { + DidNotRun, + Failed, + NoCollisionEarlyStopped, + NoCollision, + CollisionWithPenetrationInformation, + Collision + }; + + public: + CoalScalar distance_upper_bound; + Status status; + GJKVariant gjk_variant; + GJKConvergenceCriterion convergence_criterion; + GJKConvergenceCriterionType convergence_criterion_type; + + MinkowskiDiff const* shape; + Vec3s ray; + support_func_guess_t support_hint; + /// @brief The distance between the two shapes, computed by GJK. + /// If the distance is below GJK's threshold, the shapes are in collision in + /// the eyes of GJK. If `distance_upper_bound` is set to a value lower than + /// infinity, GJK will early stop as soon as it finds `distance` to be greater + /// than `distance_upper_bound`. + CoalScalar distance; + Simplex* simplex; // Pointer to the result of the last run of GJK. + + private: + // max_iteration and tolerance are made private + // because they are meant to be set by the `reset` function. + size_t max_iterations; + CoalScalar tolerance; + + SimplexV store_v[4]; + SimplexV* free_v[4]; + vertex_id_t nfree; + vertex_id_t current; + Simplex simplices[2]; + size_t iterations; + size_t iterations_momentum_stop; + + public: + /// \param max_iterations_ number of iteration before GJK returns failure. + /// \param tolerance_ precision of the algorithm. + /// + /// The tolerance argument is useful for continuous shapes and for polyhedron + /// with some vertices closer than this threshold. + /// + /// Suggested values are 100 iterations and a tolerance of 1e-6. + GJK(size_t max_iterations_, CoalScalar tolerance_) + : max_iterations(max_iterations_), tolerance(tolerance_) { + COAL_ASSERT(tolerance_ > 0, "Tolerance must be positive.", + std::invalid_argument); + initialize(); + } + + /// @brief resets the GJK algorithm, preparing it for a new run. + /// Other than the maximum number of iterations and the tolerance, + /// this function does **not** modify the parameters of the GJK algorithm. + void reset(size_t max_iterations_, CoalScalar tolerance_); + + /// @brief GJK algorithm, given the initial value guess + Status evaluate( + const MinkowskiDiff& shape, const Vec3s& guess, + const support_func_guess_t& supportHint = support_func_guess_t::Zero()); + + /// @brief apply the support function along a direction, the result is return + /// in sv + inline void getSupport(const Vec3s& d, SimplexV& sv, + support_func_guess_t& hint) const { + shape->support(d, sv.w0, sv.w1, hint); + sv.w = sv.w0 - sv.w1; + } + + /// @brief whether the simplex enclose the origin + bool encloseOrigin(); + + /// @brief get the underlying simplex using in GJK, can be used for cache in + /// next iteration + inline Simplex* getSimplex() const { return simplex; } + + /// Tells whether the closest points are available. + bool hasClosestPoints() const { return distance < distance_upper_bound; } + + /// Get the witness points on each object, and the corresponding normal. + /// @param[in] shape is the Minkowski difference of the two shapes. + /// @param[out] w0 is the witness point on shape0. + /// @param[out] w1 is the witness point on shape1. + /// @param[out] normal is the normal of the separating plane found by + /// GJK. It points from shape0 to shape1. + void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0, + Vec3s& w1, Vec3s& normal) const; + + /// @brief get the guess from current simplex + Vec3s getGuessFromSimplex() const; + + /// @brief Distance threshold for early break. + /// GJK stops when it proved the distance is more than this threshold. + /// @note The closest points will be erroneous in this case. + /// If you want the closest points, set this to infinity (the default). + void setDistanceEarlyBreak(const CoalScalar& dup) { + distance_upper_bound = dup; + } + + /// @brief Convergence check used to stop GJK when shapes are not in + /// collision. + bool checkConvergence(const Vec3s& w, const CoalScalar& rl, CoalScalar& alpha, + const CoalScalar& omega) const; + + /// @brief Get the max number of iterations of GJK. + size_t getNumMaxIterations() const { return max_iterations; } + + /// @brief Get the tolerance of GJK. + CoalScalar getTolerance() const { return tolerance; } + + /// @brief Get the number of iterations of the last run of GJK. + size_t getNumIterations() const { return iterations; } + + /// @brief Get GJK number of iterations before momentum stops. + /// Only usefull if the Nesterov or Polyak acceleration activated. + size_t getNumIterationsMomentumStopped() const { + return iterations_momentum_stop; + } + + private: + /// @brief Initializes the GJK algorithm. + /// This function should only be called by the constructor. + /// Otherwise use \ref reset. + void initialize(); + + /// @brief discard one vertex from the simplex + inline void removeVertex(Simplex& simplex); + + /// @brief append one vertex to the simplex + inline void appendVertex(Simplex& simplex, const Vec3s& v, + support_func_guess_t& hint); + + /// @brief Project origin (0) onto line a-b + /// For a detailed explanation of how to efficiently project onto a simplex, + /// check out Ericson's book, page 403: + /// https://realtimecollisiondetection.net/ To sum up, a simplex has a voronoi + /// region for each feature it has (vertex, edge, face). We find the voronoi + /// region in which the origin lies and stop as soon as we find it; we then + /// project onto it and return the result. We start by voronoi regions + /// generated by vertices then move on to edges then faces. Checking voronoi + /// regions is done using simple dot products. Moreover, edges voronoi checks + /// reuse computations of vertices voronoi checks. The same goes for faces + /// which reuse checks from edges. + /// Finally, in addition to the voronoi procedure, checks relying on the order + /// of construction + /// of the simplex are added. To know more about these, visit + /// https://caseymuratori.com/blog_0003. + bool projectLineOrigin(const Simplex& current, Simplex& next); + + /// @brief Project origin (0) onto triangle a-b-c + /// See \ref projectLineOrigin for an explanation on simplex projections. + bool projectTriangleOrigin(const Simplex& current, Simplex& next); + + /// @brief Project origin (0) onto tetrahedron a-b-c-d + /// See \ref projectLineOrigin for an explanation on simplex projections. + bool projectTetrahedraOrigin(const Simplex& current, Simplex& next); +}; + +/// @brief class for EPA algorithm +struct COAL_DLLAPI EPA { + typedef GJK::SimplexV SimplexVertex; + struct COAL_DLLAPI SimplexFace { + Vec3s n; + CoalScalar d; + bool ignore; // If the origin does not project inside the face, we + // ignore this face. + size_t vertex_id[3]; // Index of vertex in sv_store. + SimplexFace* adjacent_faces[3]; // A face has three adjacent faces. + SimplexFace* prev_face; // The previous face in the list. + SimplexFace* next_face; // The next face in the list. + size_t adjacent_edge[3]; // Each face has 3 edges: `0`, `1` and `2`. + // The i-th adjacent face is bound (to this face) + // along its `adjacent_edge[i]`-th edge + // (with 0 <= i <= 2). + size_t pass; + + SimplexFace() : n(Vec3s::Zero()), ignore(false) {}; + }; + + /// @brief The simplex list of EPA is a linked list of faces. + /// Note: EPA's linked list does **not** own any memory. + /// The memory it refers to is contiguous and owned by a std::vector. + struct COAL_DLLAPI SimplexFaceList { + SimplexFace* root; + size_t count; + SimplexFaceList() : root(nullptr), count(0) {} + + void reset() { + root = nullptr; + count = 0; + } + + void append(SimplexFace* face) { + face->prev_face = nullptr; + face->next_face = root; + if (root != nullptr) root->prev_face = face; + root = face; + ++count; + } + + void remove(SimplexFace* face) { + if (face->next_face != nullptr) + face->next_face->prev_face = face->prev_face; + if (face->prev_face != nullptr) + face->prev_face->next_face = face->next_face; + if (face == root) root = face->next_face; + --count; + } + }; + + /// @brief We bind the face `fa` along its edge `ea` to the face `fb` along + /// its edge `fb`. + static inline void bind(SimplexFace* fa, size_t ea, SimplexFace* fb, + size_t eb) { + assert(ea == 0 || ea == 1 || ea == 2); + assert(eb == 0 || eb == 1 || eb == 2); + fa->adjacent_edge[ea] = eb; + fa->adjacent_faces[ea] = fb; + fb->adjacent_edge[eb] = ea; + fb->adjacent_faces[eb] = fa; + } + + struct COAL_DLLAPI SimplexHorizon { + SimplexFace* current_face; // current face in the horizon + SimplexFace* first_face; // first face in the horizon + size_t num_faces; // number of faces in the horizon + SimplexHorizon() + : current_face(nullptr), first_face(nullptr), num_faces(0) {} + }; + + enum Status { + DidNotRun = -1, + Failed = 0, + Valid = 1, + AccuracyReached = 1 << 1 | Valid, + Degenerated = 1 << 1 | Failed, + NonConvex = 2 << 1 | Failed, + InvalidHull = 3 << 1 | Failed, + OutOfFaces = 4 << 1 | Failed, + OutOfVertices = 5 << 1 | Failed, + FallBack = 6 << 1 | Failed + }; + + public: + Status status; + GJK::Simplex result; + Vec3s normal; + support_func_guess_t support_hint; + CoalScalar depth; + SimplexFace* closest_face; + + private: + // max_iteration and tolerance are made private + // because they are meant to be set by the `reset` function. + size_t max_iterations; + CoalScalar tolerance; + + std::vector sv_store; + std::vector fc_store; + SimplexFaceList hull, stock; + size_t num_vertices; // number of vertices in polytpoe constructed by EPA + size_t iterations; + + public: + EPA(size_t max_iterations_, CoalScalar tolerance_) + : max_iterations(max_iterations_), tolerance(tolerance_) { + initialize(); + } + + /// @brief Copy constructor of EPA. + /// Mostly needed for the copy constructor of `GJKSolver`. + EPA(const EPA& other) + : max_iterations(other.max_iterations), + tolerance(other.tolerance), + sv_store(other.sv_store), + fc_store(other.fc_store) { + initialize(); + } + + /// @brief Get the max number of iterations of EPA. + size_t getNumMaxIterations() const { return max_iterations; } + + /// @brief Get the max number of vertices of EPA. + size_t getNumMaxVertices() const { return sv_store.size(); } + + /// @brief Get the max number of faces of EPA. + size_t getNumMaxFaces() const { return fc_store.size(); } + + /// @brief Get the tolerance of EPA. + CoalScalar getTolerance() const { return tolerance; } + + /// @brief Get the number of iterations of the last run of EPA. + size_t getNumIterations() const { return iterations; } + + /// @brief Get the number of vertices in the polytope of the last run of EPA. + size_t getNumVertices() const { return num_vertices; } + + /// @brief Get the number of faces in the polytope of the last run of EPA. + size_t getNumFaces() const { return hull.count; } + + /// @brief resets the EPA algorithm, preparing it for a new run. + /// It potentially reallocates memory for the vertices and faces + /// if the passed parameters are bigger than the previous ones. + /// This function does **not** modify the parameters of the EPA algorithm, + /// i.e. the maximum number of iterations and the tolerance. + /// @note calling this function destroys the previous state of EPA. + /// In the future, we may want to copy it instead, i.e. when EPA will + /// be (properly) warm-startable. + void reset(size_t max_iterations, CoalScalar tolerance); + + /// \return a Status which can be demangled using (status & Valid) or + /// (status & Failed). The other values provide a more detailled + /// status + Status evaluate(GJK& gjk, const Vec3s& guess); + + /// Get the witness points on each object, and the corresponding normal. + /// @param[in] shape is the Minkowski difference of the two shapes. + /// @param[out] w0 is the witness point on shape0. + /// @param[out] w1 is the witness point on shape1. + /// @param[in] normal is the normal found by EPA. It points from shape0 to + /// shape1. The normal is used to correct the witness points on the shapes if + /// the shapes have a non-zero swept-sphere radius. + void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0, + Vec3s& w1, Vec3s& normal) const; + + private: + /// @brief Allocates memory for the EPA algorithm. + /// This function should only be called by the constructor. + /// Otherwise use \ref reset. + void initialize(); + + bool getEdgeDist(SimplexFace* face, const SimplexVertex& a, + const SimplexVertex& b, CoalScalar& dist); + + /// @brief Add a new face to the polytope. + /// This function sets the `ignore` flag to `true` if the origin does not + /// project inside the face. + SimplexFace* newFace(size_t id_a, size_t id_b, size_t id_vertex, + bool force = false); + + /// @brief Find the best polytope face to split + SimplexFace* findClosestFace(); + + /// @brief the goal is to add a face connecting vertex w and face edge f[e] + bool expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e, + SimplexHorizon& horizon); + + // @brief Use this function to debug expand if needed. + // void PrintExpandLooping(const SimplexFace* f, const SimplexVertex& w); +}; + +} // namespace details + +} // namespace coal + +#endif diff --git a/include/coal/narrowphase/minkowski_difference.h b/include/coal/narrowphase/minkowski_difference.h new file mode 100644 index 000000000..1d9e36bfa --- /dev/null +++ b/include/coal/narrowphase/minkowski_difference.h @@ -0,0 +1,186 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2021-2024, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ + +#ifndef COAL_MINKOWSKI_DIFFERENCE_H +#define COAL_MINKOWSKI_DIFFERENCE_H + +#include "coal/shape/geometric_shapes.h" +#include "coal/math/transform.h" +#include "coal/narrowphase/support_functions.h" + +namespace coal { + +namespace details { + +/// @brief Minkowski difference class of two shapes +/// +/// @note The Minkowski difference is expressed in the frame of the first shape. +struct COAL_DLLAPI MinkowskiDiff { + typedef Eigen::Array Array2d; + + /// @brief points to two shapes + const ShapeBase* shapes[2]; + + /// @brief Store temporary data for the computation of the support point for + /// each shape. + ShapeSupportData data[2]; + + /// @brief rotation from shape1 to shape0 + /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$. + Matrix3s oR1; + + /// @brief translation from shape1 to shape0 + /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$. + Vec3s ot1; + + /// @brief The radii of the sphere swepted around each shape of the Minkowski + /// difference. The 2 values correspond to the swept-sphere radius of shape 0 + /// and shape 1. + Array2d swept_sphere_radius; + + /// @brief Wether or not to use the normalize heuristic in the GJK Nesterov + /// acceleration. This setting is only applied if the Nesterov acceleration in + /// the GJK class is active. + bool normalize_support_direction; + + typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff, + const Vec3s& dir, Vec3s& support0, + Vec3s& support1, + support_func_guess_t& hint, + ShapeSupportData data[2]); + GetSupportFunction getSupportFunc; + + MinkowskiDiff() : normalize_support_direction(false), getSupportFunc(NULL) {} + + /// @brief Set the two shapes, assuming the relative transformation between + /// them is identity. + /// Consequently, all support points computed with the MinkowskiDiff + /// will be expressed in the world frame. + /// @param shape0 the first shape. + /// @param shape1 the second shape. + /// @tparam SupportOptions is a value of the SupportOptions enum. If set to + /// `WithSweptSphere`, the support computation will take into account the + /// swept sphere radius of the shapes. If set to `NoSweptSphere`, where + /// this information is simply stored in the Minkowski's difference + /// `swept_sphere_radius` array. This array is then used to correct the + /// solution found when GJK or EPA have converged. + /// + /// @note In practice, there is no need to take into account the swept-sphere + /// radius in the iterations of GJK/EPA. It is in fact detrimental to the + /// convergence of both algos. This is because it makes corners and edges of + /// shapes look strictly convex to the algorithms, which leads to poor + /// convergence. This swept sphere template parameter is only here for + /// debugging purposes and for specific uses cases where the swept sphere + /// radius is needed in the support function. The rule is simple. When + /// interacting with GJK/EPA, the `SupportOptions` template + /// parameter should **always** be set to `NoSweptSphere` (except for + /// debugging or testing purposes). When working directly with the shapes + /// involved in the collision, and not relying on GJK/EPA, the + /// `SupportOptions` template parameter should be set to `WithSweptSphere`. + /// This is for example the case for specialized collision/distance functions. + template + void set(const ShapeBase* shape0, const ShapeBase* shape1); + + /// @brief Set the two shapes, with a relative transformation from shape0 to + /// shape1. Consequently, all support points computed with the MinkowskiDiff + /// will be expressed in the frame of shape0. + /// @param shape0 the first shape. + /// @param shape1 the second shape. + /// @param tf0 the transformation of the first shape. + /// @param tf1 the transformation of the second shape. + /// @tparam `SupportOptions` see `set(const ShapeBase*, const + /// ShapeBase*)` for more details. + template + void set(const ShapeBase* shape0, const ShapeBase* shape1, + const Transform3s& tf0, const Transform3s& tf1); + + /// @brief support function for shape0. + /// The output vector is expressed in the local frame of shape0. + /// @return a point which belongs to the set {argmax_{v in shape0} + /// v.dot(dir)}, i.e a support of shape0 in direction dir. + /// @param dir support direction. + /// @param hint used to initialize the search when shape is a ConvexBase + /// object. + /// @tparam `SupportOptions` see `set(const ShapeBase*, const + /// ShapeBase*)` for more details. + template + inline Vec3s support0(const Vec3s& dir, int& hint) const { + return getSupport<_SupportOptions>(shapes[0], dir, hint); + } + + /// @brief support function for shape1. + /// The output vector is expressed in the local frame of shape0. + /// This is mandatory because in the end we are interested in support points + /// of the Minkowski difference; hence the supports of shape0 and shape1 must + /// live in the same frame. + /// @return a point which belongs to the set {tf * argmax_{v in shape1} + /// v.dot(R^T * dir)}, i.e. the support of shape1 in direction dir (tf is the + /// tranform from shape1 to shape0). + /// @param dir support direction. + /// @param hint used to initialize the search when shape is a ConvexBase. + /// @tparam `SupportOptions` see `set(const ShapeBase*, const + /// ShapeBase*)` for more details. + template + inline Vec3s support1(const Vec3s& dir, int& hint) const { + // clang-format off + return oR1 * getSupport<_SupportOptions>(shapes[1], oR1.transpose() * dir, hint) + ot1; + // clang-format on + } + + /// @brief Support function for the pair of shapes. This method assumes `set` + /// has already been called. + /// @param[in] dir the support direction. + /// @param[out] supp0 support of shape0 in direction dir, expressed in the + /// frame of shape0. + /// @param[out] supp1 support of shape1 in direction -dir, expressed in the + /// frame of shape0. + /// @param[in/out] hint used to initialize the search when shape is a + /// ConvexBase object. + inline void support(const Vec3s& dir, Vec3s& supp0, Vec3s& supp1, + support_func_guess_t& hint) const { + assert(getSupportFunc != NULL); + getSupportFunc(*this, dir, supp0, supp1, hint, + const_cast(data)); + } +}; + +} // namespace details + +} // namespace coal + +#endif // COAL_MINKOWSKI_DIFFERENCE_H diff --git a/include/coal/narrowphase/narrowphase.h b/include/coal/narrowphase/narrowphase.h new file mode 100644 index 000000000..65cd31b13 --- /dev/null +++ b/include/coal/narrowphase/narrowphase.h @@ -0,0 +1,727 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2018-2019, Centre National de la Recherche Scientifique + * All rights reserved. + * Copyright (c) 2021-2024, INRIA + * All rights reserved. + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan, Florent Lamiraux */ + +#ifndef COAL_NARROWPHASE_H +#define COAL_NARROWPHASE_H + +#include + +#include "coal/narrowphase/gjk.h" +#include "coal/collision_data.h" +#include "coal/narrowphase/narrowphase_defaults.h" +#include "coal/logging.h" + +namespace coal { + +/// @brief collision and distance solver based on the GJK and EPA algorithms. +/// Originally, GJK and EPA were implemented in fcl which itself took +/// inspiration from the code of the GJK in bullet. Since then, both GJK and EPA +/// have been largely modified to be faster and more robust to numerical +/// accuracy and edge cases. +struct COAL_DLLAPI GJKSolver { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// @brief GJK algorithm + mutable details::GJK gjk; + + /// @brief maximum number of iterations of GJK + size_t gjk_max_iterations; + + /// @brief tolerance of GJK + CoalScalar gjk_tolerance; + + /// @brief which warm start to use for GJK + GJKInitialGuess gjk_initial_guess; + + /// @brief Whether smart guess can be provided + /// @Deprecated Use gjk_initial_guess instead + COAL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead) + bool enable_cached_guess; + + /// @brief smart guess + mutable Vec3s cached_guess; + + /// @brief smart guess for the support function + mutable support_func_guess_t support_func_cached_guess; + + /// @brief If GJK can guarantee that the distance between the shapes is + /// greater than this value, it will early stop. + CoalScalar distance_upper_bound; + + /// @brief Variant of the GJK algorithm (Default, Nesterov or Polyak). + GJKVariant gjk_variant; + + /// @brief Convergence criterion for GJK + GJKConvergenceCriterion gjk_convergence_criterion; + + /// @brief Absolute or relative convergence criterion for GJK + GJKConvergenceCriterionType gjk_convergence_criterion_type; + + /// @brief EPA algorithm + mutable details::EPA epa; + + /// @brief maximum number of iterations of EPA + size_t epa_max_iterations; + + /// @brief tolerance of EPA + CoalScalar epa_tolerance; + + /// @brief Minkowski difference used by GJK and EPA algorithms + mutable details::MinkowskiDiff minkowski_difference; + + private: + // Used internally for assertion checks. + static constexpr CoalScalar m_dummy_precision = 1e-6; + + public: + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + /// @brief Default constructor for GJK algorithm + /// By default, we don't want EPA to allocate memory because + /// certain functions of the `GJKSolver` class have specializations + /// which don't use EPA (and/or GJK). + /// So we give EPA's constructor a max number of iterations of zero. + /// Only the functions that need EPA will reset the algorithm and allocate + /// memory if needed. + GJKSolver() + : gjk(GJK_DEFAULT_MAX_ITERATIONS, GJK_DEFAULT_TOLERANCE), + gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS), + gjk_tolerance(GJK_DEFAULT_TOLERANCE), + gjk_initial_guess(GJKInitialGuess::DefaultGuess), + enable_cached_guess(false), // Use gjk_initial_guess instead + cached_guess(Vec3s(1, 0, 0)), + support_func_cached_guess(support_func_guess_t::Zero()), + distance_upper_bound((std::numeric_limits::max)()), + gjk_variant(GJKVariant::DefaultGJK), + gjk_convergence_criterion(GJKConvergenceCriterion::Default), + gjk_convergence_criterion_type(GJKConvergenceCriterionType::Absolute), + epa(0, EPA_DEFAULT_TOLERANCE), + epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS), + epa_tolerance(EPA_DEFAULT_TOLERANCE) {} + + /// @brief Constructor from a DistanceRequest + /// + /// \param[in] request DistanceRequest input + /// + /// See the default constructor; by default, we don't want + /// EPA to allocate memory so we call EPA's constructor with 0 max + /// number of iterations. + /// However, the `set` method stores the actual values of the request. + /// EPA will thus allocate memory only if needed. + explicit GJKSolver(const DistanceRequest& request) + : gjk(request.gjk_max_iterations, request.gjk_tolerance), + epa(0, request.epa_tolerance) { + this->cached_guess = Vec3s(1, 0, 0); + this->support_func_cached_guess = support_func_guess_t::Zero(); + + set(request); + } + + /// @brief setter from a DistanceRequest + /// + /// \param[in] request DistanceRequest input + /// + void set(const DistanceRequest& request) { + // --------------------- + // GJK settings + this->gjk_initial_guess = request.gjk_initial_guess; + this->enable_cached_guess = request.enable_cached_gjk_guess; + if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess || + this->enable_cached_guess) { + this->cached_guess = request.cached_gjk_guess; + this->support_func_cached_guess = request.cached_support_func_guess; + } + this->gjk_max_iterations = request.gjk_max_iterations; + this->gjk_tolerance = request.gjk_tolerance; + // For distance computation, we don't want GJK to early stop + this->distance_upper_bound = (std::numeric_limits::max)(); + this->gjk_variant = request.gjk_variant; + this->gjk_convergence_criterion = request.gjk_convergence_criterion; + this->gjk_convergence_criterion_type = + request.gjk_convergence_criterion_type; + + // --------------------- + // EPA settings + this->epa_max_iterations = request.epa_max_iterations; + this->epa_tolerance = request.epa_tolerance; + + // --------------------- + // Reset GJK and EPA status + this->epa.status = details::EPA::Status::DidNotRun; + this->gjk.status = details::GJK::Status::DidNotRun; + } + + /// @brief Constructor from a CollisionRequest + /// + /// \param[in] request CollisionRequest input + /// + /// See the default constructor; by default, we don't want + /// EPA to allocate memory so we call EPA's constructor with 0 max + /// number of iterations. + /// However, the `set` method stores the actual values of the request. + /// EPA will thus allocate memory only if needed. + explicit GJKSolver(const CollisionRequest& request) + : gjk(request.gjk_max_iterations, request.gjk_tolerance), + epa(0, request.epa_tolerance) { + this->cached_guess = Vec3s(1, 0, 0); + this->support_func_cached_guess = support_func_guess_t::Zero(); + + set(request); + } + + /// @brief setter from a CollisionRequest + /// + /// \param[in] request CollisionRequest input + /// + void set(const CollisionRequest& request) { + // --------------------- + // GJK settings + this->gjk_initial_guess = request.gjk_initial_guess; + this->enable_cached_guess = request.enable_cached_gjk_guess; + if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess || + this->enable_cached_guess) { + this->cached_guess = request.cached_gjk_guess; + this->support_func_cached_guess = request.cached_support_func_guess; + } + this->gjk_tolerance = request.gjk_tolerance; + this->gjk_max_iterations = request.gjk_max_iterations; + // The distance upper bound should be at least greater to the requested + // security margin. Otherwise, we will likely miss some collisions. + this->distance_upper_bound = (std::max)( + 0., (std::max)(request.distance_upper_bound, request.security_margin)); + this->gjk_variant = request.gjk_variant; + this->gjk_convergence_criterion = request.gjk_convergence_criterion; + this->gjk_convergence_criterion_type = + request.gjk_convergence_criterion_type; + + // --------------------- + // EPA settings + this->epa_max_iterations = request.epa_max_iterations; + this->epa_tolerance = request.epa_tolerance; + + // --------------------- + // Reset GJK and EPA status + this->gjk.status = details::GJK::Status::DidNotRun; + this->epa.status = details::EPA::Status::DidNotRun; + } + + /// @brief Copy constructor + GJKSolver(const GJKSolver& other) = default; + + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + bool operator==(const GJKSolver& other) const { + return this->enable_cached_guess == + other.enable_cached_guess && // use gjk_initial_guess instead + this->cached_guess == other.cached_guess && + this->support_func_cached_guess == other.support_func_cached_guess && + this->gjk_max_iterations == other.gjk_max_iterations && + this->gjk_tolerance == other.gjk_tolerance && + this->distance_upper_bound == other.distance_upper_bound && + this->gjk_variant == other.gjk_variant && + this->gjk_convergence_criterion == other.gjk_convergence_criterion && + this->gjk_convergence_criterion_type == + other.gjk_convergence_criterion_type && + this->gjk_initial_guess == other.gjk_initial_guess && + this->epa_max_iterations == other.epa_max_iterations && + this->epa_tolerance == other.epa_tolerance; + } + COAL_COMPILER_DIAGNOSTIC_POP + + bool operator!=(const GJKSolver& other) const { return !(*this == other); } + + /// @brief Helper to return the precision of the solver on the distance + /// estimate, depending on whether or not `compute_penetration` is true. + CoalScalar getDistancePrecision(const bool compute_penetration) const { + return compute_penetration + ? (std::max)(this->gjk_tolerance, this->epa_tolerance) + : this->gjk_tolerance; + } + + /// @brief Uses GJK and EPA to compute the distance between two shapes. + /// @param `s1` the first shape. + /// @param `tf1` the transformation of the first shape. + /// @param `s2` the second shape. + /// @param `tf2` the transformation of the second shape. + /// @param `compute_penetration` if true and GJK finds the shape in collision, + /// the EPA algorithm is also ran to compute penetration information. + /// @param[out] `p1` the witness point on the first shape. + /// @param[out] `p2` the witness point on the second shape. + /// @param[out] `normal` the normal of the collision, pointing from the first + /// to the second shape. + /// @return the estimate of the distance between the two shapes. + /// + /// @note: if `this->distance_upper_bound` is set to a positive value, GJK + /// will early stop if it finds the distance to be above this value. The + /// distance returned by `this->shapeDistance` will be a lower bound on the + /// distance between the two shapes. + /// + /// @note: the variables `this->gjk.status` and `this->epa.status` can be used + /// to examine the status of GJK and EPA. + /// + /// @note: GJK and EPA give an estimate of the distance between the two + /// shapes. This estimate is precise up to the tolerance of the algorithms: + /// - If `compute_penetration` is false, the distance is precise up to + /// `gjk_tolerance`. + /// - If `compute_penetration` is true, the distance is precise up to + /// `std::max(gjk_tolerance, epa_tolerance)` + /// It's up to the user to decide whether the shapes are in collision or not, + /// based on that estimate. + template + CoalScalar shapeDistance(const S1& s1, const Transform3s& tf1, const S2& s2, + const Transform3s& tf2, + const bool compute_penetration, Vec3s& p1, Vec3s& p2, + Vec3s& normal) const { + constexpr bool relative_transformation_already_computed = false; + CoalScalar distance; + this->runGJKAndEPA(s1, tf1, s2, tf2, compute_penetration, distance, p1, p2, + normal, relative_transformation_already_computed); + return distance; + } + + /// @brief Partial specialization of `shapeDistance` for the case where the + /// second shape is a triangle. It is more efficient to pre-compute the + /// relative transformation between the two shapes before calling GJK/EPA. + template + CoalScalar shapeDistance(const S1& s1, const Transform3s& tf1, + const TriangleP& s2, const Transform3s& tf2, + const bool compute_penetration, Vec3s& p1, Vec3s& p2, + Vec3s& normal) const { + const Transform3s tf_1M2(tf1.inverseTimes(tf2)); + TriangleP tri(tf_1M2.transform(s2.a), tf_1M2.transform(s2.b), + tf_1M2.transform(s2.c)); + + constexpr bool relative_transformation_already_computed = true; + CoalScalar distance; + this->runGJKAndEPA(s1, tf1, tri, tf_1M2, compute_penetration, distance, p1, + p2, normal, relative_transformation_already_computed); + return distance; + } + + /// @brief See other partial template specialization of shapeDistance above. + template + CoalScalar shapeDistance(const TriangleP& s1, const Transform3s& tf1, + const S2& s2, const Transform3s& tf2, + const bool compute_penetration, Vec3s& p1, Vec3s& p2, + Vec3s& normal) const { + CoalScalar distance = this->shapeDistance( + s2, tf2, s1, tf1, compute_penetration, p2, p1, normal); + normal = -normal; + return distance; + } + + protected: + /// @brief initialize GJK. + /// This method assumes `minkowski_difference` has been set. + template + void getGJKInitialGuess(const S1& s1, const S2& s2, Vec3s& guess, + support_func_guess_t& support_hint, + const Vec3s& default_guess = Vec3s(1, 0, 0)) const { + // There is no reason not to warm-start the support function, so we always + // do it. + support_hint = this->support_func_cached_guess; + // The following switch takes care of the GJK warm-start. + switch (gjk_initial_guess) { + case GJKInitialGuess::DefaultGuess: + guess = default_guess; + break; + case GJKInitialGuess::CachedGuess: + guess = this->cached_guess; + break; + case GJKInitialGuess::BoundingVolumeGuess: + if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) { + COAL_THROW_PRETTY( + "computeLocalAABB must have been called on the shapes before " + "using " + "GJKInitialGuess::BoundingVolumeGuess.", + std::logic_error); + } + guess.noalias() = + s1.aabb_local.center() - + (this->minkowski_difference.oR1 * s2.aabb_local.center() + + this->minkowski_difference.ot1); + break; + default: + COAL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error); + } + // TODO: use gjk_initial_guess instead + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + if (this->enable_cached_guess) { + guess = this->cached_guess; + } + COAL_COMPILER_DIAGNOSTIC_POP + } + + /// @brief Runs the GJK algorithm. + /// @param `s1` the first shape. + /// @param `tf1` the transformation of the first shape. + /// @param `s2` the second shape. + /// @param `tf2` the transformation of the second shape. + /// @param `compute_penetration` if true and if the shapes are in found in + /// collision, the EPA algorithm is also ran to compute penetration + /// information. + /// @param[out] `distance` the distance between the two shapes. + /// @param[out] `p1` the witness point on the first shape. + /// @param[out] `p2` the witness point on the second shape. + /// @param[out] `normal` the normal of the collision, pointing from the first + /// to the second shape. + /// @param `relative_transformation_already_computed` whether the relative + /// transformation between the two shapes has already been computed. + /// @tparam SupportOptions, see `MinkowskiDiff::set`. Whether the support + /// computations should take into account the shapes' swept-sphere radii + /// during the iterations of GJK and EPA. Please leave this default value to + /// `false` unless you know what you are doing. This template parameter is + /// only used for debugging/testing purposes. In short, there is no need to + /// take into account the swept sphere radius when computing supports in the + /// iterations of GJK and EPA. GJK and EPA will correct the solution once they + /// have converged. + /// @return the estimate of the distance between the two shapes. + /// + /// @note: The variables `this->gjk.status` and `this->epa.status` can be used + /// to examine the status of GJK and EPA. + template + void runGJKAndEPA( + const S1& s1, const Transform3s& tf1, const S2& s2, + const Transform3s& tf2, const bool compute_penetration, + CoalScalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal, + const bool relative_transformation_already_computed = false) const { + // Reset internal state of GJK algorithm + if (relative_transformation_already_computed) + this->minkowski_difference.set<_SupportOptions>(&s1, &s2); + else + this->minkowski_difference.set<_SupportOptions>(&s1, &s2, tf1, tf2); + this->gjk.reset(this->gjk_max_iterations, this->gjk_tolerance); + this->gjk.setDistanceEarlyBreak(this->distance_upper_bound); + this->gjk.gjk_variant = this->gjk_variant; + this->gjk.convergence_criterion = this->gjk_convergence_criterion; + this->gjk.convergence_criterion_type = this->gjk_convergence_criterion_type; + this->epa.status = details::EPA::Status::DidNotRun; + + // Get initial guess for GJK: default, cached or bounding volume guess + Vec3s guess; + support_func_guess_t support_hint; + getGJKInitialGuess(*(this->minkowski_difference.shapes[0]), + *(this->minkowski_difference.shapes[1]), guess, + support_hint); + + this->gjk.evaluate(this->minkowski_difference, guess, support_hint); + + switch (this->gjk.status) { + case details::GJK::DidNotRun: + COAL_ASSERT(false, "GJK did not run. It should have!", + std::logic_error); + this->cached_guess = Vec3s(1, 0, 0); + this->support_func_cached_guess.setZero(); + distance = -(std::numeric_limits::max)(); + p1 = p2 = normal = + Vec3s::Constant(std::numeric_limits::quiet_NaN()); + break; + case details::GJK::Failed: + // + // GJK ran out of iterations. + COAL_LOG_WARNING("GJK ran out of iterations."); + GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); + break; + case details::GJK::NoCollisionEarlyStopped: + // + // Case where GJK early stopped because the distance was found to be + // above the `distance_upper_bound`. + // The two witness points have no meaning. + GJKEarlyStopExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + COAL_ASSERT(distance >= this->gjk.distance_upper_bound - + this->m_dummy_precision, + "The distance should be bigger than GJK's " + "`distance_upper_bound`.", + std::logic_error); + break; + case details::GJK::NoCollision: + // + // Case where GJK converged and proved that the shapes are not in + // collision, i.e their distance is above GJK's tolerance (default + // 1e-6). + GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); + COAL_ASSERT(std::abs((p1 - p2).norm() - distance) <= + this->gjk.getTolerance() + this->m_dummy_precision, + "The distance found by GJK should coincide with the " + "distance between the closest points.", + std::logic_error); + break; + // + // Next are the cases where GJK found the shapes to be in collision, i.e. + // their distance is below GJK's tolerance (default 1e-6). + case details::GJK::CollisionWithPenetrationInformation: + GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); + COAL_ASSERT( + distance <= this->gjk.getTolerance() + this->m_dummy_precision, + "The distance found by GJK should be negative or at " + "least below GJK's tolerance.", + std::logic_error); + break; + case details::GJK::Collision: + if (!compute_penetration) { + // Skip EPA and set the witness points and the normal to nans. + GJKCollisionExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + } else { + // + // GJK was not enough to recover the penetration information. + // We need to run the EPA algorithm to find the witness points, + // penetration depth and the normal. + + // Reset EPA algorithm. Potentially allocate memory if + // `epa_max_face_num` or `epa_max_vertex_num` are bigger than EPA's + // current storage. + this->epa.reset(this->epa_max_iterations, this->epa_tolerance); + + // TODO: understand why EPA's performance is so bad on cylinders and + // cones. + this->epa.evaluate(this->gjk, -guess); + + switch (epa.status) { + // + // In the following switch cases, until the "Valid" case, + // EPA either ran out of iterations, of faces or of vertices. + // The depth, witness points and the normal are still valid, + // simply not at the precision of EPA's tolerance. + // The flag `COAL_ENABLE_LOGGING` enables feebdack on these + // cases. + // + // TODO: Remove OutOfFaces and OutOfVertices statuses and simply + // compute the upper bound on max faces and max vertices as a + // function of the number of iterations. + case details::EPA::OutOfFaces: + COAL_LOG_WARNING("EPA ran out of faces."); + EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); + break; + case details::EPA::OutOfVertices: + COAL_LOG_WARNING("EPA ran out of vertices."); + EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); + break; + case details::EPA::Failed: + COAL_LOG_WARNING("EPA ran out of iterations."); + EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); + break; + case details::EPA::Valid: + case details::EPA::AccuracyReached: + COAL_ASSERT( + -epa.depth <= epa.getTolerance() + this->m_dummy_precision, + "EPA's penetration distance should be negative (or " + "at least below EPA's tolerance).", + std::logic_error); + EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); + break; + case details::EPA::Degenerated: + COAL_LOG_WARNING( + "EPA warning: created a polytope with a degenerated face."); + EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); + break; + case details::EPA::NonConvex: + COAL_LOG_WARNING( + "EPA warning: EPA got called onto non-convex shapes."); + EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); + break; + case details::EPA::InvalidHull: + COAL_LOG_WARNING("EPA warning: created an invalid polytope."); + EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); + break; + case details::EPA::DidNotRun: + COAL_ASSERT(false, "EPA did not run. It should have!", + std::logic_error); + COAL_LOG_ERROR("EPA error: did not run. It should have."); + EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + break; + case details::EPA::FallBack: + COAL_ASSERT( + false, + "EPA went into fallback mode. It should never do that.", + std::logic_error); + COAL_LOG_ERROR("EPA error: FallBack."); + EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + break; + } + } + break; // End of case details::GJK::Collision + } + } + + void GJKEarlyStopExtractWitnessPointsAndNormal(const Transform3s& tf1, + CoalScalar& distance, + Vec3s& p1, Vec3s& p2, + Vec3s& normal) const { + COAL_UNUSED_VARIABLE(tf1); + // Cache gjk result for potential future call to this GJKSolver. + this->cached_guess = this->gjk.ray; + this->support_func_cached_guess = this->gjk.support_hint; + + distance = this->gjk.distance; + p1 = p2 = normal = + Vec3s::Constant(std::numeric_limits::quiet_NaN()); + // If we absolutely want to return some witness points, we could use + // the following code (or simply merge the early stopped case with the + // valid case below): + // gjk.getWitnessPointsAndNormal(minkowski_difference, p1, p2, normal); + // p1 = tf1.transform(p1); + // p2 = tf1.transform(p2); + // normal = tf1.getRotation() * normal; + } + + void GJKExtractWitnessPointsAndNormal(const Transform3s& tf1, + CoalScalar& distance, Vec3s& p1, + Vec3s& p2, Vec3s& normal) const { + // Apart from early stopping, there are two cases where GJK says there is no + // collision: + // 1. GJK proved the distance is above its tolerance (default 1e-6). + // 2. GJK ran out of iterations. + // In any case, `gjk.ray`'s norm is bigger than GJK's tolerance and thus + // it can safely be normalized. + COAL_ASSERT(this->gjk.ray.norm() > + this->gjk.getTolerance() - this->m_dummy_precision, + "The norm of GJK's ray should be bigger than GJK's tolerance.", + std::logic_error); + // Cache gjk result for potential future call to this GJKSolver. + this->cached_guess = this->gjk.ray; + this->support_func_cached_guess = this->gjk.support_hint; + + distance = this->gjk.distance; + // TODO: On degenerated case, the closest points may be non-unique. + // (i.e. an object face normal is colinear to `gjk.ray`) + gjk.getWitnessPointsAndNormal(this->minkowski_difference, p1, p2, normal); + Vec3s p = tf1.transform(0.5 * (p1 + p2)); + normal = tf1.getRotation() * normal; + p1.noalias() = p - 0.5 * distance * normal; + p2.noalias() = p + 0.5 * distance * normal; + } + + void GJKCollisionExtractWitnessPointsAndNormal(const Transform3s& tf1, + CoalScalar& distance, + Vec3s& p1, Vec3s& p2, + Vec3s& normal) const { + COAL_UNUSED_VARIABLE(tf1); + COAL_ASSERT(this->gjk.distance <= + this->gjk.getTolerance() + this->m_dummy_precision, + "The distance should be lower than GJK's tolerance.", + std::logic_error); + // Because GJK has returned the `Collision` status and EPA has not run, + // we purposefully do not cache the result of GJK, because ray is zero. + // However, we can cache the support function hint. + // this->cached_guess = this->gjk.ray; + this->support_func_cached_guess = this->gjk.support_hint; + + distance = this->gjk.distance; + p1 = p2 = normal = + Vec3s::Constant(std::numeric_limits::quiet_NaN()); + } + + void EPAExtractWitnessPointsAndNormal(const Transform3s& tf1, + CoalScalar& distance, Vec3s& p1, + Vec3s& p2, Vec3s& normal) const { + // Cache EPA result for potential future call to this GJKSolver. + // This caching allows to warm-start the next GJK call. + this->cached_guess = -(this->epa.depth * this->epa.normal); + this->support_func_cached_guess = this->epa.support_hint; + distance = (std::min)(0., -this->epa.depth); + this->epa.getWitnessPointsAndNormal(this->minkowski_difference, p1, p2, + normal); + // The following is very important to understand why EPA can sometimes + // return a normal that is not colinear to the vector $p_1 - p_2$ when + // working with tolerances like $\epsilon = 10^{-3}$. + // It can be resumed with a simple idea: + // EPA is an algorithm meant to find the penetration depth and the + // normal. It is not meant to find the closest points. + // Again, the issue here is **not** the normal, it's $p_1$ and $p_2$. + // + // More details: + // We'll denote $S_1$ and $S_2$ the two shapes, $n$ the normal and $p_1$ and + // $p_2$ the witness points. In theory, when EPA converges to $\epsilon = + // 0$, the normal and witness points verify the following property (P): + // - $p_1 \in \partial \sigma_{S_1}(n)$, + // - $p_2 \in \partial \sigma_{S_2}(-n), + // where $\sigma_{S_1}$ and $\sigma_{S_2}$ are the support functions of + // $S_1$ and $S_2$. The $\partial \sigma(n)$ simply denotes the support set + // of the support function in the direction $n$. (Note: I am leaving out the + // details of frame choice for the support function, to avoid making the + // mathematical notation too heavy.) + // --> In practice, EPA converges to $\epsilon > 0$. + // On polytopes and the likes, this does not change much and the property + // given above is still valid. + // --> However, this is very different on curved surfaces, such as + // ellipsoids, cylinders, cones, capsules etc. For these shapes, converging + // at $\epsilon = 10^{-6}$ or to $\epsilon = 10^{-3}$ does not change the + // normal much, but the property (P) given above is no longer valid, which + // means that the points $p_1$ and $p_2$ do not necessarily belong to the + // support sets in the direction of $n$ and thus $n$ and $p_1 - p_2$ are not + // colinear. + // + // Do not panic! This is fine. + // Although the property above is not verified, it's almost verified, + // meaning that $p_1$ and $p_2$ belong to support sets in directions that + // are very close to $n$. + // + // Solution to compute better $p_1$ and $p_2$: + // We compute the middle points of the current $p_1$ and $p_2$ and we use + // the normal and the distance given by EPA to compute the new $p_1$ and + // $p_2$. + Vec3s p = tf1.transform(0.5 * (p1 + p2)); + normal = tf1.getRotation() * normal; + p1.noalias() = p - 0.5 * distance * normal; + p2.noalias() = p + 0.5 * distance * normal; + } + + void EPAFailedExtractWitnessPointsAndNormal(const Transform3s& tf1, + CoalScalar& distance, Vec3s& p1, + Vec3s& p2, Vec3s& normal) const { + this->cached_guess = Vec3s(1, 0, 0); + this->support_func_cached_guess.setZero(); + + COAL_UNUSED_VARIABLE(tf1); + distance = -(std::numeric_limits::max)(); + p1 = p2 = normal = + Vec3s::Constant(std::numeric_limits::quiet_NaN()); + } +}; + +} // namespace coal + +#endif diff --git a/include/coal/narrowphase/narrowphase_defaults.h b/include/coal/narrowphase/narrowphase_defaults.h new file mode 100644 index 000000000..1fd2775a8 --- /dev/null +++ b/include/coal/narrowphase/narrowphase_defaults.h @@ -0,0 +1,65 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/// This file defines different macros used to characterize the default behavior +/// of the narrowphase algorithms GJK and EPA. + +#ifndef COAL_NARROWPHASE_DEFAULTS_H +#define COAL_NARROWPHASE_DEFAULTS_H + +#include "coal/data_types.h" + +namespace coal { + +/// GJK +constexpr size_t GJK_DEFAULT_MAX_ITERATIONS = 128; +constexpr CoalScalar GJK_DEFAULT_TOLERANCE = 1e-6; +/// Note: if the considered shapes are on the order of the meter, and the +/// convergence criterion of GJK is the default VDB criterion, +/// setting a tolerance of 1e-6 on the GJK algorithm makes it precise up to +/// the micro-meter. +/// The same is true for EPA. +constexpr CoalScalar GJK_MINIMUM_TOLERANCE = 1e-6; + +/// EPA +/// EPA build a polytope which maximum size is: +/// - `#iterations + 4` vertices +/// - `2 x #iterations + 4` faces +constexpr size_t EPA_DEFAULT_MAX_ITERATIONS = 64; +constexpr CoalScalar EPA_DEFAULT_TOLERANCE = 1e-6; +constexpr CoalScalar EPA_MINIMUM_TOLERANCE = 1e-6; + +} // namespace coal + +#endif // COAL_NARROWPHASE_DEFAULTS_H diff --git a/include/coal/narrowphase/support_functions.h b/include/coal/narrowphase/support_functions.h new file mode 100644 index 000000000..df79a1353 --- /dev/null +++ b/include/coal/narrowphase/support_functions.h @@ -0,0 +1,308 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2021-2024, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ + +#ifndef COAL_SUPPORT_FUNCTIONS_H +#define COAL_SUPPORT_FUNCTIONS_H + +#include "coal/shape/geometric_shapes.h" +#include "coal/math/transform.h" +#include "coal/collision_data.h" + +namespace coal { + +namespace details { + +/// @brief Options for the computation of support points. +/// `NoSweptSphere` option is used when the support function is called +/// by GJK or EPA. In this case, the swept sphere radius is not taken into +/// account in the support function. It is used by GJK and EPA after they have +/// converged to correct the solution. +/// `WithSweptSphere` option is used when the support function is called +/// directly by the user. In this case, the swept sphere radius is taken into +/// account in the support function. +enum SupportOptions { + NoSweptSphere = 0, + WithSweptSphere = 0x1, +}; + +// ============================================================================ +// ============================ SUPPORT FUNCTIONS ============================= +// ============================================================================ +/// @brief the support function for shape. +/// The output support point is expressed in the local frame of the shape. +/// @return a point which belongs to the set {argmax_{v in shape} v.dot(dir)}. +/// @param shape the shape. +/// @param dir support direction. +/// @param hint used to initialize the search when shape is a ConvexBase object. +/// @tparam SupportOptions is a value of the SupportOptions enum. If set to +/// `WithSweptSphere`, the support functions take into account the shapes' swept +/// sphere radii. Please see `MinkowskiDiff::set(const ShapeBase*, const +/// ShapeBase*)` for more details. +template +Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint); + +/// @brief Stores temporary data for the computation of support points. +struct COAL_DLLAPI ShapeSupportData { + // @brief Tracks which points have been visited in a ConvexBase. + std::vector visited; + + // @brief Tracks the last support direction used on this shape; used to + // warm-start the ConvexBase support function. + Vec3s last_dir = Vec3s::Zero(); + + // @brief Temporary set used to compute the convex-hull of a support set. + // Only used for ConvexBase and Box. + SupportSet::Polygon polygon; +}; + +/// @brief Triangle support function. +template +void getShapeSupport(const TriangleP* triangle, const Vec3s& dir, + Vec3s& support, int& /*unused*/, + ShapeSupportData& /*unused*/); + +/// @brief Box support function. +template +void getShapeSupport(const Box* box, const Vec3s& dir, Vec3s& support, + int& /*unused*/, ShapeSupportData& /*unused*/); + +/// @brief Sphere support function. +template +void getShapeSupport(const Sphere* sphere, const Vec3s& dir, Vec3s& support, + int& /*unused*/, ShapeSupportData& /*unused*/); + +/// @brief Ellipsoid support function. +template +void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3s& dir, + Vec3s& support, int& /*unused*/, + ShapeSupportData& /*unused*/); + +/// @brief Capsule support function. +template +void getShapeSupport(const Capsule* capsule, const Vec3s& dir, Vec3s& support, + int& /*unused*/, ShapeSupportData& /*unused*/); + +/// @brief Cone support function. +template +void getShapeSupport(const Cone* cone, const Vec3s& dir, Vec3s& support, + int& /*unused*/, ShapeSupportData& /*unused*/); + +/// @brief Cylinder support function. +template +void getShapeSupport(const Cylinder* cylinder, const Vec3s& dir, Vec3s& support, + int& /*unused*/, ShapeSupportData& /*unused*/); + +/// @brief ConvexBase support function. +/// @note See @ref LargeConvex and SmallConvex to see how to optimize +/// ConvexBase's support computation. +template +void getShapeSupport(const ConvexBase* convex, const Vec3s& dir, Vec3s& support, + int& hint, ShapeSupportData& /*unused*/); + +/// @brief Cast a `ConvexBase` to a `LargeConvex` to use the log version of +/// `getShapeSupport`. This is **much** faster than the linear version of +/// `getShapeSupport` when a `ConvexBase` has more than a few dozen of vertices. +/// @note WARNING: when using a LargeConvex, the neighbors in `ConvexBase` must +/// have been constructed! Otherwise the support function will segfault. +struct LargeConvex : ShapeBase {}; +/// @brief See @ref LargeConvex. +struct SmallConvex : ShapeBase {}; + +/// @brief Support function for large ConvexBase (>32 vertices). +template +void getShapeSupport(const SmallConvex* convex, const Vec3s& dir, + Vec3s& support, int& hint, ShapeSupportData& data); + +/// @brief Support function for small ConvexBase (<32 vertices). +template +void getShapeSupport(const LargeConvex* convex, const Vec3s& dir, + Vec3s& support, int& hint, ShapeSupportData& support_data); + +// ============================================================================ +// ========================== SUPPORT SET FUNCTIONS =========================== +// ============================================================================ +/// @brief Computes the support set for shape. +/// This function assumes the frame of the support set has already been +/// computed and that this frame is expressed w.r.t the local frame of the +/// shape (i.e. the local frame of the shape is the WORLD frame of the support +/// set). The support direction used to compute the support set is the positive +/// z-axis if the support set has the DEFAULT direction; negative z-axis if it +/// has the INVERTED direction. (In short, a shape's support set is has the +/// DEFAULT direction if the shape is the first shape in a collision pair. It +/// has the INVERTED direction if the shape is the second one in the collision +/// pair). +/// @return an approximation of the set {argmax_{v in shape} v.dot(dir)}, where +/// dir is the support set's support direction. +/// The support set is a plane passing by the origin of the support set frame +/// and supported by the direction dir. As a consequence, any point added to the +/// set is automatically projected onto this plane. +/// @param[in] shape the shape. +/// @param[in/out] support_set of shape. +/// @param[in/out] hint used to initialize the search when shape is a ConvexBase +/// object. +/// @param[in] num_sampled_supports is only used for shapes with smooth +/// non-strictly convex bases like cones and cylinders (their bases are +/// circles). In such a case, if the support direction points to their base, we +/// have to choose which points we want to add to the set. This is not needed +/// for boxes or ConvexBase for example. Indeed, because their support sets are +/// always polygons, we can characterize the entire support set with the +/// vertices of the polygon. +/// @param[in] tol given a point v on the shape, if +/// `max_{p in shape}(p.dot(dir)) - v.dot(dir) <= tol`, where dir is the set's +/// support direction, then v is added to the support set. +/// Otherwise said, if a point p of the shape is at a distance `tol` from the +/// support plane, it is added to the set. Thus, `tol` can be seen as the +/// "thickness" of the support plane. +/// @tparam SupportOptions is a value of the SupportOptions enum. If set to +/// `WithSweptSphere`, the support functions take into account the shapes' swept +/// sphere radii. +template +void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint, + size_t num_sampled_supports = 6, CoalScalar tol = 1e-3); + +/// @brief Same as @ref getSupportSet(const ShapeBase*, const CoalScalar, +/// SupportSet&, const int) but also constructs the support set frame from +/// `dir`. +/// @note The support direction `dir` is expressed in the local frame of the +/// shape. +/// @note This function automatically deals with the `direction` of the +/// SupportSet. +template +void getSupportSet(const ShapeBase* shape, const Vec3s& dir, + SupportSet& support_set, int& hint, + size_t num_sampled_supports = 6, CoalScalar tol = 1e-3) { + support_set.tf.rotation() = constructOrthonormalBasisFromVector(dir); + const Vec3s& support_dir = support_set.getNormal(); + const Vec3s support = getSupport<_SupportOptions>(shape, support_dir, hint); + getSupportSet<_SupportOptions>(shape, support_set, hint, num_sampled_supports, + tol); +} + +/// @brief Triangle support set function. +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set, + int& /*unused*/, ShapeSupportData& /*unused*/, + size_t /*unused*/ num_sampled_supports = 6, + CoalScalar tol = 1e-3); + +/// @brief Box support set function. +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const Box* box, SupportSet& support_set, + int& /*unused*/, ShapeSupportData& support_data, + size_t /*unused*/ num_sampled_supports = 6, + CoalScalar tol = 1e-3); + +/// @brief Sphere support set function. +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set, + int& /*unused*/, ShapeSupportData& /*unused*/, + size_t /*unused*/ num_sampled_supports = 6, + CoalScalar /*unused*/ tol = 1e-3); + +/// @brief Ellipsoid support set function. +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set, + int& /*unused*/, ShapeSupportData& /*unused*/, + size_t /*unused*/ num_sampled_supports = 6, + CoalScalar /*unused*/ tol = 1e-3); + +/// @brief Capsule support set function. +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set, + int& /*unused*/, ShapeSupportData& /*unused*/, + size_t /*unused*/ num_sampled_supports = 6, + CoalScalar tol = 1e-3); + +/// @brief Cone support set function. +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const Cone* cone, SupportSet& support_set, + int& /*unused*/, ShapeSupportData& /*unused*/, + size_t num_sampled_supports = 6, CoalScalar tol = 1e-3); + +/// @brief Cylinder support set function. +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set, + int& /*unused*/, ShapeSupportData& /*unused*/, + size_t num_sampled_supports = 6, CoalScalar tol = 1e-3); + +/// @brief ConvexBase support set function. +/// Assumes the support set frame has already been computed. +/// @note See @ref LargeConvex and SmallConvex to see how to optimize +/// ConvexBase's support computation. +template +void getShapeSupportSet(const ConvexBase* convex, SupportSet& support_set, + int& hint, ShapeSupportData& support_data, + size_t /*unused*/ num_sampled_supports = 6, + CoalScalar tol = 1e-3); + +/// @brief Support set function for large ConvexBase (>32 vertices). +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const SmallConvex* convex, SupportSet& support_set, + int& /*unused*/, ShapeSupportData& /*unused*/, + size_t /*unused*/ num_sampled_supports = 6, + CoalScalar tol = 1e-3); + +/// @brief Support set function for small ConvexBase (<32 vertices). +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set, + int& hint, ShapeSupportData& support_data, + size_t /*unused*/ num_sampled_supports = 6, + CoalScalar tol = 1e-3); + +/// @brief Computes the convex-hull of support_set. For now, this function is +/// only needed for Box and ConvexBase. +/// @param[in] cloud data which contains the 2d points of the support set which +/// convex-hull we want to compute. +/// @param[out] 2d points of the the support set's convex-hull. +COAL_DLLAPI void computeSupportSetConvexHull(SupportSet::Polygon& cloud, + SupportSet::Polygon& cvx_hull); + +} // namespace details + +} // namespace coal + +#endif // COAL_SUPPORT_FUNCTIONS_H diff --git a/include/coal/octree.h b/include/coal/octree.h new file mode 100644 index 000000000..52c507953 --- /dev/null +++ b/include/coal/octree.h @@ -0,0 +1,340 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2022-2024, Inria + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_OCTREE_H +#define COAL_OCTREE_H + +#include + +#include +#include "coal/fwd.hh" +#include "coal/BV/AABB.h" +#include "coal/collision_object.h" + +namespace coal { + +/// @brief Octree is one type of collision geometry which can encode uncertainty +/// information in the sensor data. +class COAL_DLLAPI OcTree : public CollisionGeometry { + protected: + shared_ptr tree; + + CoalScalar default_occupancy; + + CoalScalar occupancy_threshold; + CoalScalar free_threshold; + + public: + typedef octomap::OcTreeNode OcTreeNode; + + /// @brief construct octree with a given resolution + explicit OcTree(CoalScalar resolution) + : tree(shared_ptr( + new octomap::OcTree(resolution))) { + default_occupancy = tree->getOccupancyThres(); + + // default occupancy/free threshold is consistent with default setting from + // octomap + occupancy_threshold = tree->getOccupancyThres(); + free_threshold = 0; + } + + /// @brief construct octree from octomap + explicit OcTree(const shared_ptr& tree_) + : tree(tree_) { + default_occupancy = tree->getOccupancyThres(); + + // default occupancy/free threshold is consistent with default setting from + // octomap + occupancy_threshold = tree->getOccupancyThres(); + free_threshold = 0; + } + + ///  \brief Copy constructor + OcTree(const OcTree& other) + : CollisionGeometry(other), + tree(other.tree), + default_occupancy(other.default_occupancy), + occupancy_threshold(other.occupancy_threshold), + free_threshold(other.free_threshold) {} + + /// \brief Clone *this into a new Octree + OcTree* clone() const { return new OcTree(*this); } + + /// \brief Returns the tree associated to the underlying octomap OcTree. + shared_ptr getTree() const { return tree; } + + void exportAsObjFile(const std::string& filename) const; + + /// @brief compute the AABB for the octree in its local coordinate system + void computeLocalAABB() { + typedef Eigen::Matrix Vec3sloat; + Vec3sloat max_extent, min_extent; + + octomap::OcTree::iterator it = + tree->begin((unsigned char)tree->getTreeDepth()); + octomap::OcTree::iterator end = tree->end(); + + if (it == end) return; + + { + const octomap::point3d& coord = + it.getCoordinate(); // getCoordinate returns a copy + max_extent = min_extent = Eigen::Map(&coord.x()); + for (++it; it != end; ++it) { + const octomap::point3d& coord = it.getCoordinate(); + const Vec3sloat pos = Eigen::Map(&coord.x()); + max_extent = max_extent.array().max(pos.array()); + min_extent = min_extent.array().min(pos.array()); + } + } + + // Account for the size of the boxes. + const CoalScalar resolution = tree->getResolution(); + max_extent.array() += float(resolution / 2.); + min_extent.array() -= float(resolution / 2.); + + aabb_local = + AABB(min_extent.cast(), max_extent.cast()); + aabb_center = aabb_local.center(); + aabb_radius = (aabb_local.min_ - aabb_center).norm(); + } + + /// @brief get the bounding volume for the root + AABB getRootBV() const { + CoalScalar delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2; + + // std::cout << "octree size " << delta << std::endl; + return AABB(Vec3s(-delta, -delta, -delta), Vec3s(delta, delta, delta)); + } + + /// @brief Returns the depth of the octree + unsigned int getTreeDepth() const { return tree->getTreeDepth(); } + + /// @brief Returns the size of the octree + unsigned long size() const { return tree->size(); } + + /// @brief Returns the resolution of the octree + CoalScalar getResolution() const { return tree->getResolution(); } + + /// @brief get the root node of the octree + OcTreeNode* getRoot() const { return tree->getRoot(); } + + /// @brief whether one node is completely occupied + bool isNodeOccupied(const OcTreeNode* node) const { + // return tree->isNodeOccupied(node); + return node->getOccupancy() >= occupancy_threshold; + } + + /// @brief whether one node is completely free + bool isNodeFree(const OcTreeNode* node) const { + // return false; // default no definitely free node + return node->getOccupancy() <= free_threshold; + } + + /// @brief whether one node is uncertain + bool isNodeUncertain(const OcTreeNode* node) const { + return (!isNodeOccupied(node)) && (!isNodeFree(node)); + } + + /// @brief transform the octree into a bunch of boxes; uncertainty information + /// is kept in the boxes. However, we only keep the occupied boxes (i.e., the + /// boxes whose occupied probability is higher enough). + std::vector toBoxes() const { + std::vector boxes; + boxes.reserve(tree->size() / 2); + for (octomap::OcTree::iterator + it = tree->begin((unsigned char)tree->getTreeDepth()), + end = tree->end(); + it != end; ++it) { + // if(tree->isNodeOccupied(*it)) + if (isNodeOccupied(&*it)) { + CoalScalar x = it.getX(); + CoalScalar y = it.getY(); + CoalScalar z = it.getZ(); + CoalScalar size = it.getSize(); + CoalScalar c = (*it).getOccupancy(); + CoalScalar t = tree->getOccupancyThres(); + + Vec6s box; + box << x, y, z, size, c, t; + boxes.push_back(box); + } + } + return boxes; + } + + /// \brief Returns a byte description of *this + std::vector tobytes() const { + typedef Eigen::Matrix Vec3sloat; + const size_t total_size = (tree->size() * sizeof(CoalScalar) * 3) / 2; + std::vector bytes; + bytes.reserve(total_size); + + for (octomap::OcTree::iterator + it = tree->begin((unsigned char)tree->getTreeDepth()), + end = tree->end(); + it != end; ++it) { + const Vec3s box_pos = + Eigen::Map(&it.getCoordinate().x()).cast(); + if (isNodeOccupied(&*it)) + std::copy(box_pos.data(), box_pos.data() + sizeof(CoalScalar) * 3, + std::back_inserter(bytes)); + } + + return bytes; + } + + /// @brief the threshold used to decide whether one node is occupied, this is + /// NOT the octree occupied_thresold + CoalScalar getOccupancyThres() const { return occupancy_threshold; } + + /// @brief the threshold used to decide whether one node is free, this is NOT + /// the octree free_threshold + CoalScalar getFreeThres() const { return free_threshold; } + + CoalScalar getDefaultOccupancy() const { return default_occupancy; } + + void setCellDefaultOccupancy(CoalScalar d) { default_occupancy = d; } + + void setOccupancyThres(CoalScalar d) { occupancy_threshold = d; } + + void setFreeThres(CoalScalar d) { free_threshold = d; } + + /// @return ptr to child number childIdx of node + OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) { +#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0) + return tree->getNodeChild(node, childIdx); +#else + return node->getChild(childIdx); +#endif + } + + /// @return const ptr to child number childIdx of node + const OcTreeNode* getNodeChild(const OcTreeNode* node, + unsigned int childIdx) const { +#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0) + return tree->getNodeChild(node, childIdx); +#else + return node->getChild(childIdx); +#endif + } + + /// @brief return true if the child at childIdx exists + bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const { +#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0) + return tree->nodeChildExists(node, childIdx); +#else + return node->childExists(childIdx); +#endif + } + + /// @brief return true if node has at least one child + bool nodeHasChildren(const OcTreeNode* node) const { +#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0) + return tree->nodeHasChildren(node); +#else + return node->hasChildren(); +#endif + } + + /// @brief return object type, it is an octree + OBJECT_TYPE getObjectType() const { return OT_OCTREE; } + + /// @brief return node type, it is an octree + NODE_TYPE getNodeType() const { return GEOM_OCTREE; } + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const OcTree* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const OcTree& other = *other_ptr; + + return (tree.get() == other.tree.get() || toBoxes() == other.toBoxes()) && + default_occupancy == other.default_occupancy && + occupancy_threshold == other.occupancy_threshold && + free_threshold == other.free_threshold; + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// @brief compute the bounding volume of an octree node's i-th child +static inline void computeChildBV(const AABB& root_bv, unsigned int i, + AABB& child_bv) { + if (i & 1) { + child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5; + child_bv.max_[0] = root_bv.max_[0]; + } else { + child_bv.min_[0] = root_bv.min_[0]; + child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5; + } + + if (i & 2) { + child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5; + child_bv.max_[1] = root_bv.max_[1]; + } else { + child_bv.min_[1] = root_bv.min_[1]; + child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5; + } + + if (i & 4) { + child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5; + child_bv.max_[2] = root_bv.max_[2]; + } else { + child_bv.min_[2] = root_bv.min_[2]; + child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5; + } +} + +/// +/// \brief Build an OcTree from a point cloud and a given resolution +/// +/// \param[in] point_cloud The input points to insert in the OcTree +/// \param[in] resolution of the octree. +/// +/// \returns An OcTree that can be used for collision checking and more. +/// +COAL_DLLAPI OcTreePtr_t +makeOctree(const Eigen::Matrix& point_cloud, + const CoalScalar resolution); + +} // namespace coal + +#endif diff --git a/include/coal/serialization/AABB.h b/include/coal/serialization/AABB.h new file mode 100644 index 000000000..d68b2c54f --- /dev/null +++ b/include/coal/serialization/AABB.h @@ -0,0 +1,23 @@ +// +// Copyright (c) 2021 INRIA +// + +#ifndef COAL_SERIALIZATION_AABB_H +#define COAL_SERIALIZATION_AABB_H + +#include "coal/BV/AABB.h" +#include "coal/serialization/fwd.h" + +namespace boost { +namespace serialization { + +template +void serialize(Archive& ar, coal::AABB& aabb, const unsigned int /*version*/) { + ar& make_nvp("min_", aabb.min_); + ar& make_nvp("max_", aabb.max_); +} + +} // namespace serialization +} // namespace boost + +#endif // ifndef COAL_SERIALIZATION_AABB_H diff --git a/include/coal/serialization/BVH_model.h b/include/coal/serialization/BVH_model.h new file mode 100644 index 000000000..0b59a2197 --- /dev/null +++ b/include/coal/serialization/BVH_model.h @@ -0,0 +1,250 @@ +// +// Copyright (c) 2021-2022 INRIA +// + +#ifndef COAL_SERIALIZATION_BVH_MODEL_H +#define COAL_SERIALIZATION_BVH_MODEL_H + +#include "coal/BVH/BVH_model.h" + +#include "coal/serialization/fwd.h" +#include "coal/serialization/BV_node.h" +#include "coal/serialization/BV_splitter.h" +#include "coal/serialization/collision_object.h" +#include "coal/serialization/memory.h" +#include "coal/serialization/triangle.h" + +namespace boost { +namespace serialization { + +namespace internal { +struct BVHModelBaseAccessor : coal::BVHModelBase { + typedef coal::BVHModelBase Base; + using Base::num_tris_allocated; + using Base::num_vertices_allocated; +}; +} // namespace internal + +template +void save(Archive &ar, const coal::BVHModelBase &bvh_model, + const unsigned int /*version*/) { + using namespace coal; + if (!(bvh_model.build_state == BVH_BUILD_STATE_PROCESSED || + bvh_model.build_state == BVH_BUILD_STATE_UPDATED) && + (bvh_model.getModelType() == BVH_MODEL_TRIANGLES)) { + COAL_THROW_PRETTY( + "The BVH model is not in a BVH_BUILD_STATE_PROCESSED or " + "BVH_BUILD_STATE_UPDATED state.\n" + "The BVHModel could not be serialized.", + std::invalid_argument); + } + + ar &make_nvp( + "base", + boost::serialization::base_object(bvh_model)); + + ar &make_nvp("num_vertices", bvh_model.num_vertices); + ar &make_nvp("vertices", bvh_model.vertices); + + ar &make_nvp("num_tris", bvh_model.num_tris); + ar &make_nvp("tri_indices", bvh_model.tri_indices); + ar &make_nvp("build_state", bvh_model.build_state); + + ar &make_nvp("prev_vertices", bvh_model.prev_vertices); + + // if(bvh_model.convex) + // { + // const bool has_convex = true; + // ar << make_nvp("has_convex",has_convex); + // } + // else + // { + // const bool has_convex = false; + // ar << make_nvp("has_convex",has_convex); + // } +} + +template +void load(Archive &ar, coal::BVHModelBase &bvh_model, + const unsigned int /*version*/) { + using namespace coal; + + ar >> make_nvp("base", + boost::serialization::base_object( + bvh_model)); + + ar >> make_nvp("num_vertices", bvh_model.num_vertices); + ar >> make_nvp("vertices", bvh_model.vertices); + + ar >> make_nvp("num_tris", bvh_model.num_tris); + ar >> make_nvp("tri_indices", bvh_model.tri_indices); + ar >> make_nvp("build_state", bvh_model.build_state); + + ar >> make_nvp("prev_vertices", bvh_model.prev_vertices); + + // bool has_convex = true; + // ar >> make_nvp("has_convex",has_convex); +} + +COAL_SERIALIZATION_SPLIT(coal::BVHModelBase) + +namespace internal { +template +struct BVHModelAccessor : coal::BVHModel { + typedef coal::BVHModel Base; + using Base::bvs; + using Base::num_bvs; + using Base::num_bvs_allocated; + using Base::primitive_indices; +}; +} // namespace internal + +template +void serialize(Archive &ar, coal::BVHModel &bvh_model, + const unsigned int version) { + split_free(ar, bvh_model, version); +} + +template +void save(Archive &ar, const coal::BVHModel &bvh_model_, + const unsigned int /*version*/) { + using namespace coal; + typedef internal::BVHModelAccessor Accessor; + typedef BVNode Node; + + const Accessor &bvh_model = reinterpret_cast(bvh_model_); + ar &make_nvp("base", + boost::serialization::base_object(bvh_model)); + + // if(bvh_model.primitive_indices) + // { + // const bool with_primitive_indices = true; + // ar & make_nvp("with_primitive_indices",with_primitive_indices); + // + // int num_primitives = 0; + // switch(bvh_model.getModelType()) + // { + // case BVH_MODEL_TRIANGLES: + // num_primitives = bvh_model.num_tris; + // break; + // case BVH_MODEL_POINTCLOUD: + // num_primitives = bvh_model.num_vertices; + // break; + // default: + // ; + // } + // + // ar & make_nvp("num_primitives",num_primitives); + // if(num_primitives > 0) + // { + // typedef Eigen::Matrix + // AsPrimitiveIndexVector; const Eigen::Map + // primitive_indices_map(reinterpret_cast(bvh_model.primitive_indices),1,num_primitives); ar & + // make_nvp("primitive_indices",primitive_indices_map); + //// ar & + /// make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives)); + // } + // } + // else + // { + // const bool with_primitive_indices = false; + // ar & make_nvp("with_primitive_indices",with_primitive_indices); + // } + // + + if (bvh_model.bvs.get()) { + const bool with_bvs = true; + ar &make_nvp("with_bvs", with_bvs); + ar &make_nvp("num_bvs", bvh_model.num_bvs); + ar &make_nvp( + "bvs", + make_array( + reinterpret_cast(bvh_model.bvs->data()), + sizeof(Node) * + (std::size_t)bvh_model.num_bvs)); // Assuming BVs are POD. + } else { + const bool with_bvs = false; + ar &make_nvp("with_bvs", with_bvs); + } +} + +template +void load(Archive &ar, coal::BVHModel &bvh_model_, + const unsigned int /*version*/) { + using namespace coal; + typedef internal::BVHModelAccessor Accessor; + typedef BVNode Node; + + Accessor &bvh_model = reinterpret_cast(bvh_model_); + + ar >> make_nvp("base", + boost::serialization::base_object(bvh_model)); + + // bool with_primitive_indices; + // ar >> make_nvp("with_primitive_indices",with_primitive_indices); + // if(with_primitive_indices) + // { + // int num_primitives; + // ar >> make_nvp("num_primitives",num_primitives); + // + // delete[] bvh_model.primitive_indices; + // if(num_primitives > 0) + // { + // bvh_model.primitive_indices = new unsigned int[num_primitives]; + // ar & + // make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives)); + // } + // else + // bvh_model.primitive_indices = NULL; + // } + + bool with_bvs; + ar >> make_nvp("with_bvs", with_bvs); + if (with_bvs) { + unsigned int num_bvs; + ar >> make_nvp("num_bvs", num_bvs); + + if (num_bvs != bvh_model.num_bvs) { + bvh_model.bvs.reset(); + bvh_model.num_bvs = num_bvs; + if (num_bvs > 0) + bvh_model.bvs.reset(new + typename BVHModel::bv_node_vector_t(num_bvs)); + } + if (num_bvs > 0) { + ar >> make_nvp("bvs", + make_array(reinterpret_cast(bvh_model.bvs->data()), + sizeof(Node) * (std::size_t)num_bvs)); + } else + bvh_model.bvs.reset(); + } +} + +} // namespace serialization +} // namespace boost + +namespace coal { + +namespace internal { +template +struct memory_footprint_evaluator<::coal::BVHModel> { + static size_t run(const ::coal::BVHModel &bvh_model) { + return static_cast(bvh_model.memUsage(false)); + } +}; +} // namespace internal + +} // namespace coal + +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::AABB>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::OBB>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::RSS>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::OBBRSS>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::kIOS>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<16>>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<18>>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<24>>) + +#endif // ifndef COAL_SERIALIZATION_BVH_MODEL_H diff --git a/include/coal/serialization/BV_node.h b/include/coal/serialization/BV_node.h new file mode 100644 index 000000000..6e207ee66 --- /dev/null +++ b/include/coal/serialization/BV_node.h @@ -0,0 +1,35 @@ +// +// Copyright (c) 2021 INRIA +// + +#ifndef COAL_SERIALIZATION_BV_NODE_H +#define COAL_SERIALIZATION_BV_NODE_H + +#include "coal/BV/BV_node.h" + +#include "coal/serialization/fwd.h" +#include "coal/serialization/OBBRSS.h" + +namespace boost { +namespace serialization { + +template +void serialize(Archive& ar, coal::BVNodeBase& node, + const unsigned int /*version*/) { + ar& make_nvp("first_child", node.first_child); + ar& make_nvp("first_primitive", node.first_primitive); + ar& make_nvp("num_primitives", node.num_primitives); +} + +template +void serialize(Archive& ar, coal::BVNode& node, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object(node)); + ar& make_nvp("bv", node.bv); +} + +} // namespace serialization +} // namespace boost + +#endif // ifndef COAL_SERIALIZATION_BV_NODE_H diff --git a/include/coal/serialization/BV_splitter.h b/include/coal/serialization/BV_splitter.h new file mode 100644 index 000000000..9f6d7e98f --- /dev/null +++ b/include/coal/serialization/BV_splitter.h @@ -0,0 +1,62 @@ +// +// Copyright (c) 2021 INRIA +// + +#ifndef COAL_SERIALIZATION_BV_SPLITTER_H +#define COAL_SERIALIZATION_BV_SPLITTER_H + +#include "coal/internal/BV_splitter.h" + +#include "coal/serialization/fwd.h" + +namespace boost { +namespace serialization { + +namespace internal { +template +struct BVSplitterAccessor : coal::BVSplitter { + typedef coal::BVSplitter Base; + using Base::split_axis; + using Base::split_method; + using Base::split_value; + using Base::split_vector; + using Base::tri_indices; + using Base::type; + using Base::vertices; +}; +} // namespace internal + +template +void save(Archive &ar, const coal::BVSplitter &splitter_, + const unsigned int /*version*/) { + using namespace coal; + typedef internal::BVSplitterAccessor Accessor; + const Accessor &splitter = reinterpret_cast(splitter_); + + ar &make_nvp("split_axis", splitter.split_axis); + ar &make_nvp("split_vector", splitter.split_vector); + ar &make_nvp("split_value", splitter.split_value); + ar &make_nvp("type", splitter.type); + ar &make_nvp("split_method", splitter.split_method); +} + +template +void load(Archive &ar, coal::BVSplitter &splitter_, + const unsigned int /*version*/) { + using namespace coal; + typedef internal::BVSplitterAccessor Accessor; + Accessor &splitter = reinterpret_cast(splitter_); + + ar >> make_nvp("split_axis", splitter.split_axis); + ar >> make_nvp("split_vector", splitter.split_vector); + ar >> make_nvp("split_value", splitter.split_value); + ar >> make_nvp("type", splitter.type); + ar >> make_nvp("split_method", splitter.split_method); + + splitter.vertices = NULL; + splitter.tri_indices = NULL; +} +} // namespace serialization +} // namespace boost + +#endif // ifndef COAL_SERIALIZATION_BV_SPLITTER_H diff --git a/include/coal/serialization/OBB.h b/include/coal/serialization/OBB.h new file mode 100644 index 000000000..298ec15a6 --- /dev/null +++ b/include/coal/serialization/OBB.h @@ -0,0 +1,25 @@ +// +// Copyright (c) 2021 INRIA +// + +#ifndef COAL_SERIALIZATION_OBB_H +#define COAL_SERIALIZATION_OBB_H + +#include "coal/BV/OBB.h" + +#include "coal/serialization/fwd.h" + +namespace boost { +namespace serialization { + +template +void serialize(Archive& ar, coal::OBB& bv, const unsigned int /*version*/) { + ar& make_nvp("axes", bv.axes); + ar& make_nvp("To", bv.To); + ar& make_nvp("extent", bv.extent); +} + +} // namespace serialization +} // namespace boost + +#endif // ifndef COAL_SERIALIZATION_OBB_H diff --git a/include/coal/serialization/OBBRSS.h b/include/coal/serialization/OBBRSS.h new file mode 100644 index 000000000..990587c7f --- /dev/null +++ b/include/coal/serialization/OBBRSS.h @@ -0,0 +1,26 @@ +// +// Copyright (c) 2021 INRIA +// + +#ifndef COAL_SERIALIZATION_OBBRSS_H +#define COAL_SERIALIZATION_OBBRSS_H + +#include "coal/BV/OBBRSS.h" + +#include "coal/serialization/fwd.h" +#include "coal/serialization/OBB.h" +#include "coal/serialization/RSS.h" + +namespace boost { +namespace serialization { + +template +void serialize(Archive& ar, coal::OBBRSS& bv, const unsigned int /*version*/) { + ar& make_nvp("obb", bv.obb); + ar& make_nvp("rss", bv.rss); +} + +} // namespace serialization +} // namespace boost + +#endif // ifndef COAL_SERIALIZATION_OBBRSS_H diff --git a/include/coal/serialization/RSS.h b/include/coal/serialization/RSS.h new file mode 100644 index 000000000..0e317c1f9 --- /dev/null +++ b/include/coal/serialization/RSS.h @@ -0,0 +1,26 @@ +// +// Copyright (c) 2021 INRIA +// + +#ifndef COAL_SERIALIZATION_RSS_H +#define COAL_SERIALIZATION_RSS_H + +#include "coal/BV/RSS.h" + +#include "coal/serialization/fwd.h" + +namespace boost { +namespace serialization { + +template +void serialize(Archive& ar, coal::RSS& bv, const unsigned int /*version*/) { + ar& make_nvp("axes", bv.axes); + ar& make_nvp("Tr", bv.Tr); + ar& make_nvp("length", make_array(bv.length, 2)); + ar& make_nvp("radius", bv.radius); +} + +} // namespace serialization +} // namespace boost + +#endif // ifndef COAL_SERIALIZATION_RSS_H diff --git a/include/coal/serialization/archive.h b/include/coal/serialization/archive.h new file mode 100644 index 000000000..9d7a410c7 --- /dev/null +++ b/include/coal/serialization/archive.h @@ -0,0 +1,292 @@ +// +// Copyright (c) 2017-2024 CNRS INRIA +// This file was borrowed from the Pinocchio library: +// https://github.com/stack-of-tasks/pinocchio +// + +#ifndef COAL_SERIALIZATION_ARCHIVE_H +#define COAL_SERIALIZATION_ARCHIVE_H + +#include "coal/fwd.hh" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#if BOOST_VERSION / 100 % 1000 == 78 && __APPLE__ +// See https://github.com/qcscine/utilities/issues/5#issuecomment-1246897049 for +// further details + +#ifndef BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC +#define DEFINE_BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC +#define BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC +#endif + +#include + +#ifdef DEFINE_BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC +#undef BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC +#endif + +#else +#include +#endif + +#include +#include +#include + +// Handle NAN inside TXT or XML archives +#include + +namespace coal { +namespace serialization { + +/// +/// \brief Loads an object from a TXT file. +/// +/// \tparam T Type of the object to deserialize. +/// +/// \param[out] object Object in which the loaded data are copied. +/// \param[in] filename Name of the file containing the serialized data. +/// +template +inline void loadFromText(T& object, const std::string& filename) { + std::ifstream ifs(filename.c_str()); + if (ifs) { + std::locale const new_loc(ifs.getloc(), + new boost::math::nonfinite_num_get); + ifs.imbue(new_loc); + boost::archive::text_iarchive ia(ifs, boost::archive::no_codecvt); + ia >> object; + } else { + const std::string exception_message(filename + + " does not seem to be a valid file."); + throw std::invalid_argument(exception_message); + } +} + +/// +/// \brief Saves an object inside a TXT file. +/// +/// \tparam T Type of the object to deserialize. +/// +/// \param[in] object Object in which the loaded data are copied. +/// \param[in] filename Name of the file containing the serialized data. +/// +template +inline void saveToText(const T& object, const std::string& filename) { + std::ofstream ofs(filename.c_str()); + if (ofs) { + boost::archive::text_oarchive oa(ofs); + oa & object; + } else { + const std::string exception_message(filename + + " does not seem to be a valid file."); + throw std::invalid_argument(exception_message); + } +} + +/// +/// \brief Loads an object from a std::stringstream. +/// +/// \tparam T Type of the object to deserialize. +/// +/// \param[out] object Object in which the loaded data are copied. +/// \param[in] is string stream constaining the serialized content of the +/// object. +/// +template +inline void loadFromStringStream(T& object, std::istringstream& is) { + std::locale const new_loc(is.getloc(), + new boost::math::nonfinite_num_get); + is.imbue(new_loc); + boost::archive::text_iarchive ia(is, boost::archive::no_codecvt); + ia >> object; +} + +/// +/// \brief Saves an object inside a std::stringstream. +/// +/// \tparam T Type of the object to deserialize. +/// +/// \param[in] object Object in which the loaded data are copied. +/// \param[out] ss String stream constaining the serialized content of the +/// object. +/// +template +inline void saveToStringStream(const T& object, std::stringstream& ss) { + boost::archive::text_oarchive oa(ss); + oa & object; +} + +/// +/// \brief Loads an object from a std::string +/// +/// \tparam T Type of the object to deserialize. +/// +/// \param[out] object Object in which the loaded data are copied. +/// \param[in] str string constaining the serialized content of the object. +/// +template +inline void loadFromString(T& object, const std::string& str) { + std::istringstream is(str); + loadFromStringStream(object, is); +} + +/// +/// \brief Saves an object inside a std::string +/// +/// \tparam T Type of the object to deserialize. +/// +/// \param[in] object Object in which the loaded data are copied. +/// +/// \returns a string constaining the serialized content of the object. +/// +template +inline std::string saveToString(const T& object) { + std::stringstream ss; + saveToStringStream(object, ss); + return ss.str(); +} + +/// +/// \brief Loads an object from a XML file. +/// +/// \tparam T Type of the object to deserialize. +/// +/// \param[out] object Object in which the loaded data are copied. +/// \param[in] filename Name of the file containing the serialized data. +/// \param[in] tag_name XML Tag for the given object. +/// +template +inline void loadFromXML(T& object, const std::string& filename, + const std::string& tag_name) { + if (filename.empty()) { + COAL_THROW_PRETTY("Tag name should not be empty.", std::invalid_argument); + } + + std::ifstream ifs(filename.c_str()); + if (ifs) { + std::locale const new_loc(ifs.getloc(), + new boost::math::nonfinite_num_get); + ifs.imbue(new_loc); + boost::archive::xml_iarchive ia(ifs, boost::archive::no_codecvt); + ia >> boost::serialization::make_nvp(tag_name.c_str(), object); + } else { + const std::string exception_message(filename + + " does not seem to be a valid file."); + throw std::invalid_argument(exception_message); + } +} + +/// +/// \brief Saves an object inside a XML file. +/// +/// \tparam T Type of the object to deserialize. +/// +/// \param[in] object Object in which the loaded data are copied. +/// \param[in] filename Name of the file containing the serialized data. +/// \param[in] tag_name XML Tag for the given object. +/// +template +inline void saveToXML(const T& object, const std::string& filename, + const std::string& tag_name) { + if (filename.empty()) { + COAL_THROW_PRETTY("Tag name should not be empty.", std::invalid_argument); + } + + std::ofstream ofs(filename.c_str()); + if (ofs) { + boost::archive::xml_oarchive oa(ofs); + oa& boost::serialization::make_nvp(tag_name.c_str(), object); + } else { + const std::string exception_message(filename + + " does not seem to be a valid file."); + throw std::invalid_argument(exception_message); + } +} + +/// +/// \brief Loads an object from a binary file. +/// +/// \tparam T Type of the object to deserialize. +/// +/// \param[out] object Object in which the loaded data are copied. +/// \param[in] filename Name of the file containing the serialized data. +/// +template +inline void loadFromBinary(T& object, const std::string& filename) { + std::ifstream ifs(filename.c_str(), std::ios::binary); + if (ifs) { + boost::archive::binary_iarchive ia(ifs); + ia >> object; + } else { + const std::string exception_message(filename + + " does not seem to be a valid file."); + throw std::invalid_argument(exception_message); + } +} + +/// +/// \brief Saves an object inside a binary file. +/// +/// \tparam T Type of the object to deserialize. +/// +/// \param[in] object Object in which the loaded data are copied. +/// \param[in] filename Name of the file containing the serialized data. +/// +template +void saveToBinary(const T& object, const std::string& filename) { + std::ofstream ofs(filename.c_str(), std::ios::binary); + if (ofs) { + boost::archive::binary_oarchive oa(ofs); + oa & object; + } else { + const std::string exception_message(filename + + " does not seem to be a valid file."); + throw std::invalid_argument(exception_message); + } +} + +/// +/// \brief Loads an object from a binary buffer. +/// +/// \tparam T Type of the object to deserialize. +/// +/// \param[out] object Object in which the loaded data are copied. +/// \param[in] buffer Input buffer containing the serialized data. +/// +template +inline void loadFromBuffer(T& object, boost::asio::streambuf& buffer) { + boost::archive::binary_iarchive ia(buffer); + ia >> object; +} + +/// +/// \brief Saves an object to a binary buffer. +/// +/// \tparam T Type of the object to serialize. +/// +/// \param[in] object Object in which the loaded data are copied. +/// \param[out] buffer Output buffer containing the serialized data. +/// +template +void saveToBuffer(const T& object, boost::asio::streambuf& buffer) { + boost::archive::binary_oarchive oa(buffer); + oa & object; +} + +} // namespace serialization +} // namespace coal + +#endif // ifndef COAL_SERIALIZATION_ARCHIVE_H diff --git a/include/coal/serialization/collision_data.h b/include/coal/serialization/collision_data.h new file mode 100644 index 000000000..0c1466dc2 --- /dev/null +++ b/include/coal/serialization/collision_data.h @@ -0,0 +1,179 @@ +// +// Copyright (c) 2021 INRIA +// + +#ifndef COAL_SERIALIZATION_COLLISION_DATA_H +#define COAL_SERIALIZATION_COLLISION_DATA_H + +#include "coal/collision_data.h" +#include "coal/serialization/fwd.h" + +namespace boost { +namespace serialization { + +template +void save(Archive& ar, const coal::Contact& contact, + const unsigned int /*version*/) { + ar& make_nvp("b1", contact.b1); + ar& make_nvp("b2", contact.b2); + ar& make_nvp("normal", contact.normal); + ar& make_nvp("nearest_points", contact.nearest_points); + ar& make_nvp("pos", contact.pos); + ar& make_nvp("penetration_depth", contact.penetration_depth); +} + +template +void load(Archive& ar, coal::Contact& contact, const unsigned int /*version*/) { + ar >> make_nvp("b1", contact.b1); + ar >> make_nvp("b2", contact.b2); + ar >> make_nvp("normal", contact.normal); + std::array nearest_points; + ar >> make_nvp("nearest_points", nearest_points); + contact.nearest_points[0] = nearest_points[0]; + contact.nearest_points[1] = nearest_points[1]; + ar >> make_nvp("pos", contact.pos); + ar >> make_nvp("penetration_depth", contact.penetration_depth); + contact.o1 = NULL; + contact.o2 = NULL; +} + +COAL_SERIALIZATION_SPLIT(coal::Contact) + +template +void serialize(Archive& ar, coal::QueryRequest& query_request, + const unsigned int /*version*/) { + ar& make_nvp("gjk_initial_guess", query_request.gjk_initial_guess); + // TODO: use gjk_initial_guess instead + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + ar& make_nvp("enable_cached_gjk_guess", + query_request.enable_cached_gjk_guess); + COAL_COMPILER_DIAGNOSTIC_POP + ar& make_nvp("cached_gjk_guess", query_request.cached_gjk_guess); + ar& make_nvp("cached_support_func_guess", + query_request.cached_support_func_guess); + ar& make_nvp("gjk_max_iterations", query_request.gjk_max_iterations); + ar& make_nvp("gjk_tolerance", query_request.gjk_tolerance); + ar& make_nvp("gjk_variant", query_request.gjk_variant); + ar& make_nvp("gjk_convergence_criterion", + query_request.gjk_convergence_criterion); + ar& make_nvp("gjk_convergence_criterion_type", + query_request.gjk_convergence_criterion_type); + ar& make_nvp("epa_max_iterations", query_request.epa_max_iterations); + ar& make_nvp("epa_tolerance", query_request.epa_tolerance); + ar& make_nvp("collision_distance_threshold", + query_request.collision_distance_threshold); + ar& make_nvp("enable_timings", query_request.enable_timings); +} + +template +void serialize(Archive& ar, coal::QueryResult& query_result, + const unsigned int /*version*/) { + ar& make_nvp("cached_gjk_guess", query_result.cached_gjk_guess); + ar& make_nvp("cached_support_func_guess", + query_result.cached_support_func_guess); +} + +template +void serialize(Archive& ar, coal::CollisionRequest& collision_request, + const unsigned int /*version*/) { + ar& make_nvp("base", boost::serialization::base_object( + collision_request)); + ar& make_nvp("num_max_contacts", collision_request.num_max_contacts); + ar& make_nvp("enable_contact", collision_request.enable_contact); + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + ar& make_nvp("enable_distance_lower_bound", + collision_request.enable_distance_lower_bound); + COAL_COMPILER_DIAGNOSTIC_POP + ar& make_nvp("security_margin", collision_request.security_margin); + ar& make_nvp("break_distance", collision_request.break_distance); + ar& make_nvp("distance_upper_bound", collision_request.distance_upper_bound); +} + +template +void save(Archive& ar, const coal::CollisionResult& collision_result, + const unsigned int /*version*/) { + ar& make_nvp("base", boost::serialization::base_object( + collision_result)); + ar& make_nvp("contacts", collision_result.getContacts()); + ar& make_nvp("distance_lower_bound", collision_result.distance_lower_bound); + ar& make_nvp("nearest_points", collision_result.nearest_points); + ar& make_nvp("normal", collision_result.normal); +} + +template +void load(Archive& ar, coal::CollisionResult& collision_result, + const unsigned int /*version*/) { + ar >> make_nvp("base", boost::serialization::base_object( + collision_result)); + std::vector contacts; + ar >> make_nvp("contacts", contacts); + collision_result.clear(); + for (size_t k = 0; k < contacts.size(); ++k) + collision_result.addContact(contacts[k]); + ar >> make_nvp("distance_lower_bound", collision_result.distance_lower_bound); + std::array nearest_points; + ar >> make_nvp("nearest_points", nearest_points); + collision_result.nearest_points[0] = nearest_points[0]; + collision_result.nearest_points[1] = nearest_points[1]; + ar >> make_nvp("normal", collision_result.normal); +} + +COAL_SERIALIZATION_SPLIT(coal::CollisionResult) + +template +void serialize(Archive& ar, coal::DistanceRequest& distance_request, + const unsigned int /*version*/) { + ar& make_nvp("base", boost::serialization::base_object( + distance_request)); + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + ar& make_nvp("enable_nearest_points", distance_request.enable_nearest_points); + COAL_COMPILER_DIAGNOSTIC_POP + ar& make_nvp("enable_signed_distance", + distance_request.enable_signed_distance); + ar& make_nvp("rel_err", distance_request.rel_err); + ar& make_nvp("abs_err", distance_request.abs_err); +} + +template +void save(Archive& ar, const coal::DistanceResult& distance_result, + const unsigned int /*version*/) { + ar& make_nvp("base", boost::serialization::base_object( + distance_result)); + ar& make_nvp("min_distance", distance_result.min_distance); + ar& make_nvp("nearest_points", distance_result.nearest_points); + ar& make_nvp("normal", distance_result.normal); + ar& make_nvp("b1", distance_result.b1); + ar& make_nvp("b2", distance_result.b2); +} + +template +void load(Archive& ar, coal::DistanceResult& distance_result, + const unsigned int /*version*/) { + ar >> make_nvp("base", boost::serialization::base_object( + distance_result)); + ar >> make_nvp("min_distance", distance_result.min_distance); + std::array nearest_points; + ar >> make_nvp("nearest_points", nearest_points); + distance_result.nearest_points[0] = nearest_points[0]; + distance_result.nearest_points[1] = nearest_points[1]; + ar >> make_nvp("normal", distance_result.normal); + ar >> make_nvp("b1", distance_result.b1); + ar >> make_nvp("b2", distance_result.b2); + distance_result.o1 = NULL; + distance_result.o2 = NULL; +} + +COAL_SERIALIZATION_SPLIT(coal::DistanceResult) + +} // namespace serialization +} // namespace boost + +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionRequest) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionResult) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::DistanceRequest) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::DistanceResult) + +#endif // ifndef COAL_SERIALIZATION_COLLISION_DATA_H diff --git a/include/coal/serialization/collision_object.h b/include/coal/serialization/collision_object.h new file mode 100644 index 000000000..c37ee465c --- /dev/null +++ b/include/coal/serialization/collision_object.h @@ -0,0 +1,93 @@ +// +// Copyright (c) 2021 INRIA +// + +#ifndef COAL_SERIALIZATION_COLLISION_OBJECT_H +#define COAL_SERIALIZATION_COLLISION_OBJECT_H + +#include "coal/collision_object.h" + +#include "coal/serialization/fwd.h" +#include "coal/serialization/AABB.h" + +namespace boost { +namespace serialization { + +template +void save(Archive& ar, const coal::CollisionGeometry& collision_geometry, + const unsigned int /*version*/) { + ar& make_nvp("aabb_center", collision_geometry.aabb_center); + ar& make_nvp("aabb_radius", collision_geometry.aabb_radius); + ar& make_nvp("aabb_local", collision_geometry.aabb_local); + ar& make_nvp("cost_density", collision_geometry.cost_density); + ar& make_nvp("threshold_occupied", collision_geometry.threshold_occupied); + ar& make_nvp("threshold_free", collision_geometry.threshold_free); +} + +template +void load(Archive& ar, coal::CollisionGeometry& collision_geometry, + const unsigned int /*version*/) { + ar >> make_nvp("aabb_center", collision_geometry.aabb_center); + ar >> make_nvp("aabb_radius", collision_geometry.aabb_radius); + ar >> make_nvp("aabb_local", collision_geometry.aabb_local); + ar >> make_nvp("cost_density", collision_geometry.cost_density); + ar >> make_nvp("threshold_occupied", collision_geometry.threshold_occupied); + ar >> make_nvp("threshold_free", collision_geometry.threshold_free); + collision_geometry.user_data = NULL; // no way to recover this +} + +COAL_SERIALIZATION_SPLIT(coal::CollisionGeometry) + +} // namespace serialization +} // namespace boost + +namespace coal { + +// fwd declaration +template +class HeightField; + +template +class Convex; + +struct OBB; +struct OBBRSS; +class AABB; + +class OcTree; +class Box; +class Sphere; +class Ellipsoid; +class Capsule; +class Cone; +class TriangleP; +class Cylinder; +class Halfspace; +class Plane; + +namespace serialization { +template <> +struct register_type { + template + static void on(Archive& ar) { + ar.template register_type(); + ar.template register_type(); + ar.template register_type(); + ar.template register_type(); + ar.template register_type(); + ar.template register_type(); + ar.template register_type(); + ar.template register_type(); + ar.template register_type(); + ar.template register_type(); + ar.template register_type>(); + ar.template register_type>(); + ar.template register_type>(); + ar.template register_type>(); + ; + } +}; +} // namespace serialization +} // namespace coal + +#endif // ifndef COAL_SERIALIZATION_COLLISION_OBJECT_H diff --git a/include/coal/serialization/contact_patch.h b/include/coal/serialization/contact_patch.h new file mode 100644 index 000000000..aae049daa --- /dev/null +++ b/include/coal/serialization/contact_patch.h @@ -0,0 +1,87 @@ +// +// Copyright (c) 2024 INRIA +// + +#ifndef COAL_SERIALIZATION_CONTACT_PATCH_H +#define COAL_SERIALIZATION_CONTACT_PATCH_H + +#include "coal/collision_data.h" +#include "coal/serialization/fwd.h" +#include "coal/serialization/transform.h" + +namespace boost { +namespace serialization { + +template +void serialize(Archive& ar, coal::ContactPatch& contact_patch, + const unsigned int /*version*/) { + using namespace coal; + typedef Eigen::Matrix PolygonPoints; + + size_t patch_size = contact_patch.size(); + ar& make_nvp("patch_size", patch_size); + if (patch_size > 0) { + if (Archive::is_loading::value) { + contact_patch.points().resize(patch_size); + } + Eigen::Map points_map( + reinterpret_cast(contact_patch.points().data()), 2, + static_cast(patch_size)); + ar& make_nvp("points", points_map); + } + + ar& make_nvp("penetration_depth", contact_patch.penetration_depth); + ar& make_nvp("direction", contact_patch.direction); + ar& make_nvp("tf", contact_patch.tf); +} + +template +void serialize(Archive& ar, coal::ContactPatchRequest& request, + const unsigned int /*version*/) { + using namespace coal; + + ar& make_nvp("max_num_patch", request.max_num_patch); + + size_t num_samples_curved_shapes = request.getNumSamplesCurvedShapes(); + CoalScalar patch_tolerance = request.getPatchTolerance(); + ar& make_nvp("num_samples_curved_shapes", num_samples_curved_shapes); + ar& make_nvp("patch_tolerance", num_samples_curved_shapes); + + if (Archive::is_loading::value) { + request.setNumSamplesCurvedShapes(num_samples_curved_shapes); + request.setPatchTolerance(patch_tolerance); + } +} + +template +void serialize(Archive& ar, coal::ContactPatchResult& result, + const unsigned int /*version*/) { + using namespace coal; + + size_t num_patches = result.numContactPatches(); + ar& make_nvp("num_patches", num_patches); + + std::vector patches; + patches.resize(num_patches); + if (Archive::is_loading::value) { + ar& make_nvp("patches", patches); + + const ContactPatchRequest request(num_patches); + result.set(request); + for (size_t i = 0; i < num_patches; ++i) { + ContactPatch& patch = result.getUnusedContactPatch(); + patch = patches[i]; + } + } else { + patches.clear(); + for (size_t i = 0; i < num_patches; ++i) { + patches.emplace_back(result.getContactPatch(i)); + } + ar& make_nvp("patches", patches); + } +} + +} // namespace serialization +} // namespace boost + +#endif // COAL_SERIALIZATION_CONTACT_PATCH_H diff --git a/include/coal/serialization/convex.h b/include/coal/serialization/convex.h new file mode 100644 index 000000000..5c89a918d --- /dev/null +++ b/include/coal/serialization/convex.h @@ -0,0 +1,167 @@ +// +// Copyright (c) 2022-2024 INRIA +// + +#ifndef COAL_SERIALIZATION_CONVEX_H +#define COAL_SERIALIZATION_CONVEX_H + +#include "coal/shape/convex.h" + +#include "coal/serialization/fwd.h" +#include "coal/serialization/geometric_shapes.h" +#include "coal/serialization/memory.h" +#include "coal/serialization/triangle.h" +#include "coal/serialization/quadrilateral.h" + +namespace boost { +namespace serialization { + +namespace internal { +struct ConvexBaseAccessor : coal::ConvexBase { + typedef coal::ConvexBase Base; +}; + +} // namespace internal + +template +void serialize(Archive& ar, coal::ConvexBase& convex_base, + const unsigned int /*version*/) { + using namespace coal; + + ar& make_nvp("base", + boost::serialization::base_object(convex_base)); + + const unsigned int num_points_previous = convex_base.num_points; + ar& make_nvp("num_points", convex_base.num_points); + + const unsigned int num_normals_and_offsets_previous = + convex_base.num_normals_and_offsets; + ar& make_nvp("num_normals_and_offsets", convex_base.num_normals_and_offsets); + + const int num_warm_start_supports_previous = + static_cast(convex_base.support_warm_starts.points.size()); + assert(num_warm_start_supports_previous == + static_cast(convex_base.support_warm_starts.indices.size())); + int num_warm_start_supports = num_warm_start_supports_previous; + ar& make_nvp("num_warm_start_supports", num_warm_start_supports); + + if (Archive::is_loading::value) { + if (num_points_previous != convex_base.num_points) { + convex_base.points.reset(); + if (convex_base.num_points > 0) + convex_base.points.reset( + new std::vector(convex_base.num_points)); + } + + if (num_normals_and_offsets_previous != + convex_base.num_normals_and_offsets) { + convex_base.normals.reset(); + convex_base.offsets.reset(); + if (convex_base.num_normals_and_offsets > 0) { + convex_base.normals.reset( + new std::vector(convex_base.num_normals_and_offsets)); + convex_base.offsets.reset( + new std::vector(convex_base.num_normals_and_offsets)); + } + } + + if (num_warm_start_supports_previous != num_warm_start_supports) { + convex_base.support_warm_starts.points.resize( + static_cast(num_warm_start_supports)); + convex_base.support_warm_starts.indices.resize( + static_cast(num_warm_start_supports)); + } + } + + typedef Eigen::Matrix MatrixPoints; + if (convex_base.num_points > 0) { + Eigen::Map points_map( + reinterpret_cast(convex_base.points->data()), 3, + convex_base.num_points); + ar& make_nvp("points", points_map); + } + + typedef Eigen::Matrix VecOfReals; + if (convex_base.num_normals_and_offsets > 0) { + Eigen::Map normals_map( + reinterpret_cast(convex_base.normals->data()), 3, + convex_base.num_normals_and_offsets); + ar& make_nvp("normals", normals_map); + + Eigen::Map offsets_map( + reinterpret_cast(convex_base.offsets->data()), 1, + convex_base.num_normals_and_offsets); + ar& make_nvp("offsets", offsets_map); + } + + typedef Eigen::Matrix VecOfInts; + if (num_warm_start_supports > 0) { + Eigen::Map warm_start_support_points_map( + reinterpret_cast( + convex_base.support_warm_starts.points.data()), + 3, num_warm_start_supports); + ar& make_nvp("warm_start_support_points", warm_start_support_points_map); + + Eigen::Map warm_start_support_indices_map( + reinterpret_cast(convex_base.support_warm_starts.indices.data()), + 1, num_warm_start_supports); + ar& make_nvp("warm_start_support_indices", warm_start_support_indices_map); + } + + ar& make_nvp("center", convex_base.center); + // We don't save neighbors as they will be computed directly by calling + // fillNeighbors. +} + +namespace internal { +template +struct ConvexAccessor : coal::Convex { + typedef coal::Convex Base; + using Base::fillNeighbors; +}; + +} // namespace internal + +template +void serialize(Archive& ar, coal::Convex& convex_, + const unsigned int /*version*/) { + using namespace coal; + typedef internal::ConvexAccessor Accessor; + + Accessor& convex = reinterpret_cast(convex_); + ar& make_nvp("base", boost::serialization::base_object(convex_)); + + const unsigned int num_polygons_previous = convex.num_polygons; + ar& make_nvp("num_polygons", convex.num_polygons); + + if (Archive::is_loading::value) { + if (num_polygons_previous != convex.num_polygons) { + convex.polygons.reset(new std::vector(convex.num_polygons)); + } + } + + ar& make_array(convex.polygons->data(), convex.num_polygons); + + if (Archive::is_loading::value) convex.fillNeighbors(); +} + +} // namespace serialization +} // namespace boost + +COAL_SERIALIZATION_DECLARE_EXPORT(coal::Convex) +COAL_SERIALIZATION_DECLARE_EXPORT(coal::Convex) + +namespace coal { + +// namespace internal { +// template +// struct memory_footprint_evaluator< ::coal::BVHModel > { +// static size_t run(const ::coal::BVHModel &bvh_model) { +// return static_cast(bvh_model.memUsage(false)); +// } +// }; +// } // namespace internal + +} // namespace coal + +#endif // ifndef COAL_SERIALIZATION_CONVEX_H diff --git a/include/coal/serialization/eigen.h b/include/coal/serialization/eigen.h new file mode 100644 index 000000000..24430cf02 --- /dev/null +++ b/include/coal/serialization/eigen.h @@ -0,0 +1,122 @@ +// +// Copyright (c) 2017-2021 CNRS INRIA +// + +/* + Code adapted from Pinocchio and + https://gist.githubusercontent.com/mtao/5798888/raw/5be9fa9b66336c166dba3a92c0e5b69ffdb81501/eigen_boost_serialization.hpp + Copyright (c) 2015 Michael Tao +*/ + +#ifndef COAL_SERIALIZATION_EIGEN_H +#define COAL_SERIALIZATION_EIGEN_H + +#ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL +#ifdef HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION +#define COAL_SKIP_EIGEN_BOOST_SERIALIZATION +#endif +#endif + +#ifndef COAL_SKIP_EIGEN_BOOST_SERIALIZATION + +#include + +#include +#include +#include + +// Workaround a bug in GCC >= 7 and C++17 +// ref. https://gitlab.com/libeigen/eigen/-/issues/1676 +#ifdef __GNUC__ +#if __GNUC__ >= 7 && __cplusplus >= 201703L +namespace boost { +namespace serialization { +struct U; +} +} // namespace boost +namespace Eigen { +namespace internal { +template <> +struct traits { + enum { Flags = 0 }; +}; +} // namespace internal +} // namespace Eigen +#endif +#endif + +namespace boost { +namespace serialization { + +template +void save(Archive& ar, + const Eigen::Matrix& m, + const unsigned int /*version*/) { + Eigen::DenseIndex rows(m.rows()), cols(m.cols()); + if (Rows == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(rows); + if (Cols == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(cols); + ar& make_nvp("data", make_array(m.data(), (size_t)m.size())); +} + +template +void load(Archive& ar, + Eigen::Matrix& m, + const unsigned int /*version*/) { + Eigen::DenseIndex rows = Rows, cols = Cols; + if (Rows == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(rows); + if (Cols == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(cols); + m.resize(rows, cols); + ar >> make_nvp("data", make_array(m.data(), (size_t)m.size())); +} + +template +void serialize(Archive& ar, + Eigen::Matrix& m, + const unsigned int version) { + split_free(ar, m, version); +} + +template +void save(Archive& ar, + const Eigen::Map& m, + const unsigned int /*version*/) { + Eigen::DenseIndex rows(m.rows()), cols(m.cols()); + if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic) + ar& BOOST_SERIALIZATION_NVP(rows); + if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic) + ar& BOOST_SERIALIZATION_NVP(cols); + ar& make_nvp("data", make_array(m.data(), (size_t)m.size())); +} + +template +void load(Archive& ar, Eigen::Map& m, + const unsigned int /*version*/) { + Eigen::DenseIndex rows = PlainObjectBase::RowsAtCompileTime, + cols = PlainObjectBase::ColsAtCompileTime; + if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic) + ar >> BOOST_SERIALIZATION_NVP(rows); + if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic) + ar >> BOOST_SERIALIZATION_NVP(cols); + m.resize(rows, cols); + ar >> make_nvp("data", make_array(m.data(), (size_t)m.size())); +} + +template +void serialize(Archive& ar, + Eigen::Map& m, + const unsigned int version) { + split_free(ar, m, version); +} + +} // namespace serialization +} // namespace boost +// +#endif // ifned COAL_SKIP_EIGEN_BOOST_SERIALIZATION + +#endif // ifndef COAL_SERIALIZATION_EIGEN_H diff --git a/include/coal/serialization/fwd.h b/include/coal/serialization/fwd.h new file mode 100644 index 000000000..18164b334 --- /dev/null +++ b/include/coal/serialization/fwd.h @@ -0,0 +1,113 @@ +// +// Copyright (c) 2021-2024 INRIA +// + +#ifndef COAL_SERIALIZATION_FWD_H +#define COAL_SERIALIZATION_FWD_H + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "coal/fwd.hh" +#include "coal/serialization/eigen.h" + +#define COAL_SERIALIZATION_SPLIT(Type) \ + template \ + void serialize(Archive& ar, Type& value, const unsigned int version) { \ + split_free(ar, value, version); \ + } + +#define COAL_SERIALIZATION_DECLARE_EXPORT(T) \ + BOOST_CLASS_EXPORT_KEY(T) \ + namespace boost { \ + namespace archive { \ + namespace detail { \ + namespace extra_detail { \ + template <> \ + struct init_guid { \ + static guid_initializer const& g; \ + }; \ + } \ + } \ + } \ + } \ + /**/ + +#define COAL_SERIALIZATION_DEFINE_EXPORT(T) \ + namespace boost { \ + namespace archive { \ + namespace detail { \ + namespace extra_detail { \ + guid_initializer const& init_guid::g = \ + ::boost::serialization::singleton< \ + guid_initializer >::get_mutable_instance() \ + .export_guid(); \ + } \ + } \ + } \ + } \ + /**/ + +namespace coal { +namespace serialization { +namespace detail { + +template +struct init_cast_register {}; + +template +struct cast_register_initializer { + void init(std::true_type) const { + boost::serialization::void_cast_register(); + } + + void init(std::false_type) const {} + + cast_register_initializer const& init() const { + COAL_COMPILER_DIAGNOSTIC_PUSH + _Pragma("GCC diagnostic ignored \"-Wconversion\"") + BOOST_STATIC_WARNING((std::is_base_of::value)); + COAL_COMPILER_DIAGNOSTIC_POP + init(std::is_base_of()); + return *this; + } +}; + +} // namespace detail + +template +struct register_type { + template + static void on(Archive& /*ar*/) {} +}; +} // namespace serialization +} // namespace coal + +#define COAL_SERIALIZATION_CAST_REGISTER(Derived, Base) \ + namespace coal { \ + namespace serialization { \ + namespace detail { \ + template <> \ + struct init_cast_register { \ + static cast_register_initializer const& g; \ + }; \ + cast_register_initializer const& init_cast_register< \ + Derived, Base>::g = \ + ::boost::serialization::singleton< \ + cast_register_initializer >::get_mutable_instance() \ + .init(); \ + } \ + } \ + } + +#endif // ifndef COAL_SERIALIZATION_FWD_H diff --git a/include/coal/serialization/geometric_shapes.h b/include/coal/serialization/geometric_shapes.h new file mode 100644 index 000000000..2a14614c2 --- /dev/null +++ b/include/coal/serialization/geometric_shapes.h @@ -0,0 +1,120 @@ +// +// Copyright (c) 2021-2024 INRIA +// + +#ifndef COAL_SERIALIZATION_GEOMETRIC_SHAPES_H +#define COAL_SERIALIZATION_GEOMETRIC_SHAPES_H + +#include "coal/shape/geometric_shapes.h" +#include "coal/serialization/fwd.h" +#include "coal/serialization/collision_object.h" + +namespace boost { +namespace serialization { + +template +void serialize(Archive& ar, coal::ShapeBase& shape_base, + const unsigned int /*version*/) { + ar& make_nvp( + "base", + boost::serialization::base_object(shape_base)); + ::coal::CoalScalar radius = shape_base.getSweptSphereRadius(); + ar& make_nvp("swept_sphere_radius", radius); + + if (Archive::is_loading::value) { + shape_base.setSweptSphereRadius(radius); + } +} + +template +void serialize(Archive& ar, coal::TriangleP& triangle, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object(triangle)); + ar& make_nvp("a", triangle.a); + ar& make_nvp("b", triangle.b); + ar& make_nvp("c", triangle.c); +} + +template +void serialize(Archive& ar, coal::Box& box, const unsigned int /*version*/) { + ar& make_nvp("base", boost::serialization::base_object(box)); + ar& make_nvp("halfSide", box.halfSide); +} + +template +void serialize(Archive& ar, coal::Sphere& sphere, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object(sphere)); + ar& make_nvp("radius", sphere.radius); +} + +template +void serialize(Archive& ar, coal::Ellipsoid& ellipsoid, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object(ellipsoid)); + ar& make_nvp("radii", ellipsoid.radii); +} + +template +void serialize(Archive& ar, coal::Capsule& capsule, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object(capsule)); + ar& make_nvp("radius", capsule.radius); + ar& make_nvp("halfLength", capsule.halfLength); +} + +template +void serialize(Archive& ar, coal::Cone& cone, const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object(cone)); + ar& make_nvp("radius", cone.radius); + ar& make_nvp("halfLength", cone.halfLength); +} + +template +void serialize(Archive& ar, coal::Cylinder& cylinder, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object(cylinder)); + ar& make_nvp("radius", cylinder.radius); + ar& make_nvp("halfLength", cylinder.halfLength); +} + +template +void serialize(Archive& ar, coal::Halfspace& half_space, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object(half_space)); + ar& make_nvp("n", half_space.n); + ar& make_nvp("d", half_space.d); +} + +template +void serialize(Archive& ar, coal::Plane& plane, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object(plane)); + ar& make_nvp("n", plane.n); + ar& make_nvp("d", plane.d); +} + +} // namespace serialization +} // namespace boost + +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::ShapeBase) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionGeometry) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::TriangleP) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Box) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Sphere) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Ellipsoid) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Capsule) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Cone) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Cylinder) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Halfspace) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Plane) + +#endif // ifndef COAL_SERIALIZATION_GEOMETRIC_SHAPES_H diff --git a/include/coal/serialization/hfield.h b/include/coal/serialization/hfield.h new file mode 100644 index 000000000..8ea3d7120 --- /dev/null +++ b/include/coal/serialization/hfield.h @@ -0,0 +1,80 @@ +// +// Copyright (c) 2021-2024 INRIA +// + +#ifndef COAL_SERIALIZATION_HFIELD_H +#define COAL_SERIALIZATION_HFIELD_H + +#include "coal/hfield.h" + +#include "coal/serialization/fwd.h" +#include "coal/serialization/OBBRSS.h" + +namespace boost { +namespace serialization { + +template +void serialize(Archive &ar, coal::HFNodeBase &node, + const unsigned int /*version*/) { + ar &make_nvp("first_child", node.first_child); + ar &make_nvp("x_id", node.x_id); + ar &make_nvp("x_size", node.x_size); + ar &make_nvp("y_id", node.y_id); + ar &make_nvp("y_size", node.y_size); + ar &make_nvp("max_height", node.max_height); + ar &make_nvp("contact_active_faces", node.contact_active_faces); +} + +template +void serialize(Archive &ar, coal::HFNode &node, + const unsigned int /*version*/) { + ar &make_nvp("base", + boost::serialization::base_object(node)); + ar &make_nvp("bv", node.bv); +} + +namespace internal { +template +struct HeightFieldAccessor : coal::HeightField { + typedef coal::HeightField Base; + using Base::bvs; + using Base::heights; + using Base::max_height; + using Base::min_height; + using Base::num_bvs; + using Base::x_dim; + using Base::x_grid; + using Base::y_dim; + using Base::y_grid; +}; +} // namespace internal + +template +void serialize(Archive &ar, coal::HeightField &hf_model, + const unsigned int /*version*/) { + ar &make_nvp( + "base", + boost::serialization::base_object(hf_model)); + + typedef internal::HeightFieldAccessor Accessor; + Accessor &access = reinterpret_cast(hf_model); + + ar &make_nvp("x_dim", access.x_dim); + ar &make_nvp("y_dim", access.y_dim); + ar &make_nvp("heights", access.heights); + ar &make_nvp("min_height", access.min_height); + ar &make_nvp("max_height", access.max_height); + ar &make_nvp("x_grid", access.x_grid); + ar &make_nvp("y_grid", access.y_grid); + + ar &make_nvp("bvs", access.bvs); + ar &make_nvp("num_bvs", access.num_bvs); +} +} // namespace serialization +} // namespace boost + +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::AABB>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::OBB>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::OBBRSS>) + +#endif // ifndef COAL_SERIALIZATION_HFIELD_H diff --git a/include/coal/serialization/kDOP.h b/include/coal/serialization/kDOP.h new file mode 100644 index 000000000..f4cf992c6 --- /dev/null +++ b/include/coal/serialization/kDOP.h @@ -0,0 +1,34 @@ +// +// Copyright (c) 2024 INRIA +// + +#ifndef COAL_SERIALIZATION_kDOP_H +#define COAL_SERIALIZATION_kDOP_H + +#include "coal/BV/kDOP.h" + +#include "coal/serialization/fwd.h" + +namespace boost { +namespace serialization { + +namespace internal { +template +struct KDOPAccessor : coal::KDOP { + typedef coal::KDOP Base; + using Base::dist_; +}; +} // namespace internal + +template +void serialize(Archive& ar, coal::KDOP& bv_, + const unsigned int /*version*/) { + typedef internal::KDOPAccessor Accessor; + Accessor& access = reinterpret_cast(bv_); + ar& make_nvp("distances", make_array(access.dist_.data(), N)); +} + +} // namespace serialization +} // namespace boost + +#endif // COAL_SERIALIZATION_kDOP_H diff --git a/include/coal/serialization/kIOS.h b/include/coal/serialization/kIOS.h new file mode 100644 index 000000000..18d349db2 --- /dev/null +++ b/include/coal/serialization/kIOS.h @@ -0,0 +1,57 @@ +// +// Copyright (c) 2024 INRIA +// + +#ifndef COAL_SERIALIZATION_kIOS_H +#define COAL_SERIALIZATION_kIOS_H + +#include "coal/BV/kIOS.h" + +#include "coal/serialization/OBB.h" +#include "coal/serialization/fwd.h" + +namespace boost { +namespace serialization { + +template +void serialize(Archive& ar, coal::kIOS& bv, const unsigned int version) { + split_free(ar, bv, version); +} + +template +void save(Archive& ar, const coal::kIOS& bv, const unsigned int /*version*/) { + // Number of spheres in kIOS is never larger than kIOS::kios_max_num_spheres + ar& make_nvp("num_spheres", bv.num_spheres); + + std::array centers{}; + std::array radii; + for (std::size_t i = 0; i < coal::kIOS::max_num_spheres; ++i) { + centers[i] = bv.spheres[i].o; + radii[i] = bv.spheres[i].r; + } + ar& make_nvp("centers", make_array(centers.data(), centers.size())); + ar& make_nvp("radii", make_array(radii.data(), radii.size())); + + ar& make_nvp("obb", bv.obb); +} + +template +void load(Archive& ar, coal::kIOS& bv, const unsigned int /*version*/) { + ar >> make_nvp("num_spheres", bv.num_spheres); + + std::array centers; + std::array radii; + ar >> make_nvp("centers", make_array(centers.data(), centers.size())); + ar >> make_nvp("radii", make_array(radii.data(), radii.size())); + for (std::size_t i = 0; i < coal::kIOS::max_num_spheres; ++i) { + bv.spheres[i].o = centers[i]; + bv.spheres[i].r = radii[i]; + } + + ar >> make_nvp("obb", bv.obb); +} + +} // namespace serialization +} // namespace boost + +#endif // COAL_SERIALIZATION_kIOS_H diff --git a/include/coal/serialization/memory.h b/include/coal/serialization/memory.h new file mode 100644 index 000000000..ee278bced --- /dev/null +++ b/include/coal/serialization/memory.h @@ -0,0 +1,30 @@ +// +// Copyright (c) 2021 INRIA +// + +#ifndef COAL_SERIALIZATION_MEMORY_H +#define COAL_SERIALIZATION_MEMORY_H + +namespace coal { + +namespace internal { +template +struct memory_footprint_evaluator { + static size_t run(const T &) { return sizeof(T); } +}; +} // namespace internal + +/// \brief Returns the memory footpring of the input object. +/// For POD objects, this function returns the result of sizeof(T) +/// +/// \param[in] object whose memory footprint needs to be evaluated. +/// +/// \return the memory footprint of the input object. +template +size_t computeMemoryFootprint(const T &object) { + return internal::memory_footprint_evaluator::run(object); +} + +} // namespace coal + +#endif // ifndef COAL_SERIALIZATION_MEMORY_H diff --git a/include/coal/serialization/octree.h b/include/coal/serialization/octree.h new file mode 100644 index 000000000..769532379 --- /dev/null +++ b/include/coal/serialization/octree.h @@ -0,0 +1,101 @@ +// +// Copyright (c) 2023-2024 INRIA +// + +#ifndef COAL_SERIALIZATION_OCTREE_H +#define COAL_SERIALIZATION_OCTREE_H + +#include +#include + +#include + +#include "coal/octree.h" +#include "coal/serialization/fwd.h" + +namespace boost { +namespace serialization { + +namespace internal { +struct OcTreeAccessor : coal::OcTree { + typedef coal::OcTree Base; + using Base::default_occupancy; + using Base::free_threshold; + using Base::occupancy_threshold; + using Base::tree; +}; +} // namespace internal + +template +void save_construct_data(Archive &ar, const coal::OcTree *octree_ptr, + const unsigned int /*version*/) { + const double resolution = octree_ptr->getResolution(); + ar << make_nvp("resolution", resolution); +} + +template +void save(Archive &ar, const coal::OcTree &octree, + const unsigned int /*version*/) { + typedef internal::OcTreeAccessor Accessor; + const Accessor &access = reinterpret_cast(octree); + + std::ostringstream stream; + access.tree->write(stream); + const std::string stream_str = stream.str(); + auto size = stream_str.size(); + // We can't directly serialize stream_str because it contains binary data. + // This create a bug on Windows with text_archive + ar << make_nvp("tree_data_size", size); + ar << make_nvp("tree_data", + make_array(stream_str.c_str(), stream_str.size())); + + ar << make_nvp("base", base_object(octree)); + ar << make_nvp("default_occupancy", access.default_occupancy); + ar << make_nvp("occupancy_threshold", access.occupancy_threshold); + ar << make_nvp("free_threshold", access.free_threshold); +} + +template +void load_construct_data(Archive &ar, coal::OcTree *octree_ptr, + const unsigned int /*version*/) { + double resolution; + ar >> make_nvp("resolution", resolution); + new (octree_ptr) coal::OcTree(resolution); +} + +template +void load(Archive &ar, coal::OcTree &octree, const unsigned int /*version*/) { + typedef internal::OcTreeAccessor Accessor; + Accessor &access = reinterpret_cast(octree); + + std::size_t tree_data_size; + ar >> make_nvp("tree_data_size", tree_data_size); + + std::string stream_str; + stream_str.resize(tree_data_size); + /// TODO use stream_str.data in C++17 + assert(tree_data_size > 0 && "tree_data_size should be greater than 0"); + ar >> make_nvp("tree_data", make_array(&stream_str[0], tree_data_size)); + std::istringstream stream(stream_str); + + octomap::AbstractOcTree *new_tree = octomap::AbstractOcTree::read(stream); + access.tree = std::shared_ptr( + dynamic_cast(new_tree)); + + ar >> make_nvp("base", base_object(octree)); + ar >> make_nvp("default_occupancy", access.default_occupancy); + ar >> make_nvp("occupancy_threshold", access.occupancy_threshold); + ar >> make_nvp("free_threshold", access.free_threshold); +} + +template +void serialize(Archive &ar, coal::OcTree &octree, const unsigned int version) { + split_free(ar, octree, version); +} + +} // namespace serialization +} // namespace boost + +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::OcTree) + +#endif // ifndef COAL_SERIALIZATION_OCTREE_H diff --git a/include/coal/serialization/quadrilateral.h b/include/coal/serialization/quadrilateral.h new file mode 100644 index 000000000..b805f5bde --- /dev/null +++ b/include/coal/serialization/quadrilateral.h @@ -0,0 +1,26 @@ +// +// Copyright (c) 2022 INRIA +// + +#ifndef COAL_SERIALIZATION_QUADRILATERAL_H +#define COAL_SERIALIZATION_QUADRILATERAL_H + +#include "coal/data_types.h" +#include "coal/serialization/fwd.h" + +namespace boost { +namespace serialization { + +template +void serialize(Archive &ar, coal::Quadrilateral &quadrilateral, + const unsigned int /*version*/) { + ar &make_nvp("p0", quadrilateral[0]); + ar &make_nvp("p1", quadrilateral[1]); + ar &make_nvp("p2", quadrilateral[2]); + ar &make_nvp("p3", quadrilateral[3]); +} + +} // namespace serialization +} // namespace boost + +#endif // ifndef COAL_SERIALIZATION_QUADRILATERAL_H diff --git a/include/coal/serialization/serializer.h b/include/coal/serialization/serializer.h new file mode 100644 index 000000000..daef9fc64 --- /dev/null +++ b/include/coal/serialization/serializer.h @@ -0,0 +1,92 @@ +// +// Copyright (c) 2024 INRIA +// + +#ifndef COAL_SERIALIZATION_SERIALIZER_H +#define COAL_SERIALIZATION_SERIALIZER_H + +#include "coal/serialization/archive.h" + +namespace coal { +namespace serialization { + +struct Serializer { + /// \brief Loads an object from a text file. + template + static void loadFromText(T& object, const std::string& filename) { + ::coal::serialization::loadFromText(object, filename); + } + + /// \brief Saves an object as a text file. + template + static void saveToText(const T& object, const std::string& filename) { + ::coal::serialization::saveToText(object, filename); + } + + /// \brief Loads an object from a stream string. + template + static void loadFromStringStream(T& object, std::istringstream& is) { + ::coal::serialization::loadFromStringStream(object, is); + } + + /// \brief Saves an object to a string stream. + template + static void saveToStringStream(const T& object, std::stringstream& ss) { + ::coal::serialization::saveToStringStream(object, ss); + } + + /// \brief Loads an object from a string. + template + static void loadFromString(T& object, const std::string& str) { + ::coal::serialization::loadFromString(object, str); + } + + /// \brief Saves a Derived object to a string. + template + static std::string saveToString(const T& object) { + return ::coal::serialization::saveToString(object); + } + + /// \brief Loads an object from an XML file. + template + static void loadFromXML(T& object, const std::string& filename, + const std::string& tag_name) { + ::coal::serialization::loadFromXML(object, filename, tag_name); + } + + /// \brief Saves an object as an XML file. + template + static void saveToXML(const T& object, const std::string& filename, + const std::string& tag_name) { + ::coal::serialization::saveToXML(object, filename, tag_name); + } + + /// \brief Loads a Derived object from an binary file. + template + static void loadFromBinary(T& object, const std::string& filename) { + ::coal::serialization::loadFromBinary(object, filename); + } + + /// \brief Saves a Derived object as an binary file. + template + static void saveToBinary(const T& object, const std::string& filename) { + ::coal::serialization::saveToBinary(object, filename); + } + + /// \brief Loads an object from a binary buffer. + template + static void loadFromBuffer(T& object, boost::asio::streambuf& container) { + ::coal::serialization::loadFromBuffer(object, container); + } + + /// \brief Saves an object as a binary buffer. + template + static void saveToBuffer(const T& object, boost::asio::streambuf& container) { + ::coal::serialization::saveToBuffer(object, container); + } +}; + +} // namespace serialization +} // namespace coal + +#endif // ifndef COAL_SERIALIZATION_SERIALIZER_H diff --git a/include/coal/serialization/transform.h b/include/coal/serialization/transform.h new file mode 100644 index 000000000..c43542d7a --- /dev/null +++ b/include/coal/serialization/transform.h @@ -0,0 +1,24 @@ +// +// Copyright (c) 2024 INRIA +// + +#ifndef COAL_SERIALIZATION_TRANSFORM_H +#define COAL_SERIALIZATION_TRANSFORM_H + +#include "coal/math/transform.h" +#include "coal/serialization/fwd.h" + +namespace boost { +namespace serialization { + +template +void serialize(Archive& ar, coal::Transform3s& tf, + const unsigned int /*version*/) { + ar& make_nvp("R", tf.rotation()); + ar& make_nvp("T", tf.translation()); +} + +} // namespace serialization +} // namespace boost + +#endif // COAL_SERIALIZATION_TRANSFORM_H diff --git a/include/coal/serialization/triangle.h b/include/coal/serialization/triangle.h new file mode 100644 index 000000000..052706be5 --- /dev/null +++ b/include/coal/serialization/triangle.h @@ -0,0 +1,25 @@ +// +// Copyright (c) 2021-2022 INRIA +// + +#ifndef COAL_SERIALIZATION_TRIANGLE_H +#define COAL_SERIALIZATION_TRIANGLE_H + +#include "coal/data_types.h" +#include "coal/serialization/fwd.h" + +namespace boost { +namespace serialization { + +template +void serialize(Archive &ar, coal::Triangle &triangle, + const unsigned int /*version*/) { + ar &make_nvp("p0", triangle[0]); + ar &make_nvp("p1", triangle[1]); + ar &make_nvp("p2", triangle[2]); +} + +} // namespace serialization +} // namespace boost + +#endif // ifndef COAL_SERIALIZATION_TRIANGLE_H diff --git a/include/coal/shape/convex.h b/include/coal/shape/convex.h new file mode 100644 index 000000000..a17108a6e --- /dev/null +++ b/include/coal/shape/convex.h @@ -0,0 +1,112 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_SHAPE_CONVEX_H +#define COAL_SHAPE_CONVEX_H + +#include "coal/shape/geometric_shapes.h" + +namespace coal { + +/// @brief Convex polytope +/// @tparam PolygonT the polygon class. It must have method \c size() and +/// \c operator[](int i) +template +class Convex : public ConvexBase { + public: + /// @brief Construct an uninitialized convex object + Convex() : ConvexBase(), num_polygons(0) {} + + /// @brief Constructing a convex, providing normal and offset of each polytype + /// surface, and the points and shape topology information \param ownStorage + /// whether this class owns the pointers of points and + /// polygons. If owned, they are deleted upon destruction. + /// \param points_ list of 3D points + /// \param num_points_ number of 3D points + /// \param polygons_ \copydoc Convex::polygons + /// \param num_polygons_ the number of polygons. + /// \note num_polygons_ is not the allocated size of polygons_. + Convex(std::shared_ptr> points_, unsigned int num_points_, + std::shared_ptr> polygons_, + unsigned int num_polygons_); + + /// @brief Copy constructor + /// Only the list of neighbors is copied. + Convex(const Convex& other); + + ~Convex(); + + /// based on http://number-none.com/blow/inertia/bb_inertia.doc + Matrix3s computeMomentofInertia() const; + + Vec3s computeCOM() const; + + CoalScalar computeVolume() const; + + /// + /// @brief Set the current Convex from a list of points and polygons. + /// + /// \param ownStorage whether this class owns the pointers of points and + /// polygons. If owned, they are deleted upon destruction. + /// \param points list of 3D points + /// \param num_points number of 3D points + /// \param polygons \copydoc Convex::polygons + /// \param num_polygons the number of polygons. + /// \note num_polygons is not the allocated size of polygons. + /// + void set(std::shared_ptr> points, unsigned int num_points, + std::shared_ptr> polygons, + unsigned int num_polygons); + + /// @brief Clone (deep copy) + virtual Convex* clone() const; + + /// @brief An array of PolygonT object. + /// PolygonT should contains a list of vertices for each polygon, + /// in counter clockwise order. + std::shared_ptr> polygons; + unsigned int num_polygons; + + protected: + void fillNeighbors(); +}; + +} // namespace coal + +#include "coal/shape/details/convex.hxx" + +#endif diff --git a/include/coal/shape/details/convex.hxx b/include/coal/shape/details/convex.hxx new file mode 100644 index 000000000..0c0ccf58c --- /dev/null +++ b/include/coal/shape/details/convex.hxx @@ -0,0 +1,283 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, CNRS - LAAS + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Joseph Mirabel */ + +#ifndef COAL_SHAPE_CONVEX_HXX +#define COAL_SHAPE_CONVEX_HXX + +#include +#include +#include + +namespace coal { + +template +Convex::Convex(std::shared_ptr> points_, + unsigned int num_points_, + std::shared_ptr> polygons_, + unsigned int num_polygons_) + : ConvexBase(), polygons(polygons_), num_polygons(num_polygons_) { + this->initialize(points_, num_points_); + this->fillNeighbors(); + this->buildSupportWarmStart(); +} + +template +Convex::Convex(const Convex& other) + : ConvexBase(other), num_polygons(other.num_polygons) { + if (other.polygons.get()) { + polygons.reset(new std::vector(*(other.polygons))); + } else + polygons.reset(); +} + +template +Convex::~Convex() {} + +template +void Convex::set(std::shared_ptr> points_, + unsigned int num_points_, + std::shared_ptr> polygons_, + unsigned int num_polygons_) { + ConvexBase::set(points_, num_points_); + + this->num_polygons = num_polygons_; + this->polygons = polygons_; + + this->fillNeighbors(); + this->buildSupportWarmStart(); +} + +template +Convex* Convex::clone() const { + return new Convex(*this); +} + +template +Matrix3s Convex::computeMomentofInertia() const { + typedef typename PolygonT::size_type size_type; + typedef typename PolygonT::index_type index_type; + + Matrix3s C = Matrix3s::Zero(); + + Matrix3s C_canonical; + C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0, 1 / 120.0, + 1 / 120.0, 1 / 120.0, 1 / 60.0; + + if (!(points.get())) { + std::cerr + << "Error in `Convex::computeMomentofInertia`! Convex has no vertices." + << std::endl; + return C; + } + const std::vector& points_ = *points; + if (!(polygons.get())) { + std::cerr + << "Error in `Convex::computeMomentofInertia`! Convex has no polygons." + << std::endl; + return C; + } + const std::vector& polygons_ = *polygons; + for (unsigned int i = 0; i < num_polygons; ++i) { + const PolygonT& polygon = polygons_[i]; + + // compute the center of the polygon + Vec3s plane_center(0, 0, 0); + for (size_type j = 0; j < polygon.size(); ++j) + plane_center += points_[polygon[(index_type)j]]; + plane_center /= polygon.size(); + + // compute the volume of tetrahedron making by neighboring two points, the + // plane center and the reference point (zero) of the convex shape + const Vec3s& v3 = plane_center; + for (size_type j = 0; j < polygon.size(); ++j) { + index_type e_first = polygon[static_cast(j)]; + index_type e_second = + polygon[static_cast((j + 1) % polygon.size())]; + const Vec3s& v1 = points_[e_first]; + const Vec3s& v2 = points_[e_second]; + Matrix3s A; + A << v1.transpose(), v2.transpose(), + v3.transpose(); // this is A' in the original document + C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3); + } + } + + return C.trace() * Matrix3s::Identity() - C; +} + +template +Vec3s Convex::computeCOM() const { + typedef typename PolygonT::size_type size_type; + typedef typename PolygonT::index_type index_type; + + Vec3s com(0, 0, 0); + CoalScalar vol = 0; + if (!(points.get())) { + std::cerr << "Error in `Convex::computeCOM`! Convex has no vertices." + << std::endl; + return com; + } + const std::vector& points_ = *points; + if (!(polygons.get())) { + std::cerr << "Error in `Convex::computeCOM`! Convex has no polygons." + << std::endl; + return com; + } + const std::vector& polygons_ = *polygons; + for (unsigned int i = 0; i < num_polygons; ++i) { + const PolygonT& polygon = polygons_[i]; + // compute the center of the polygon + Vec3s plane_center(0, 0, 0); + for (size_type j = 0; j < polygon.size(); ++j) + plane_center += points_[polygon[(index_type)j]]; + plane_center /= polygon.size(); + + // compute the volume of tetrahedron making by neighboring two points, the + // plane center and the reference point (zero) of the convex shape + const Vec3s& v3 = plane_center; + for (size_type j = 0; j < polygon.size(); ++j) { + index_type e_first = polygon[static_cast(j)]; + index_type e_second = + polygon[static_cast((j + 1) % polygon.size())]; + const Vec3s& v1 = points_[e_first]; + const Vec3s& v2 = points_[e_second]; + CoalScalar d_six_vol = (v1.cross(v2)).dot(v3); + vol += d_six_vol; + com += (points_[e_first] + points_[e_second] + plane_center) * d_six_vol; + } + } + + return com / (vol * 4); // here we choose zero as the reference +} + +template +CoalScalar Convex::computeVolume() const { + typedef typename PolygonT::size_type size_type; + typedef typename PolygonT::index_type index_type; + + CoalScalar vol = 0; + if (!(points.get())) { + std::cerr << "Error in `Convex::computeVolume`! Convex has no vertices." + << std::endl; + return vol; + } + const std::vector& points_ = *points; + if (!(polygons.get())) { + std::cerr << "Error in `Convex::computeVolume`! Convex has no polygons." + << std::endl; + return vol; + } + const std::vector& polygons_ = *polygons; + for (unsigned int i = 0; i < num_polygons; ++i) { + const PolygonT& polygon = polygons_[i]; + + // compute the center of the polygon + Vec3s plane_center(0, 0, 0); + for (size_type j = 0; j < polygon.size(); ++j) + plane_center += points_[polygon[(index_type)j]]; + plane_center /= polygon.size(); + + // compute the volume of tetrahedron making by neighboring two points, the + // plane center and the reference point (zero point) of the convex shape + const Vec3s& v3 = plane_center; + for (size_type j = 0; j < polygon.size(); ++j) { + index_type e_first = polygon[static_cast(j)]; + index_type e_second = + polygon[static_cast((j + 1) % polygon.size())]; + const Vec3s& v1 = points_[e_first]; + const Vec3s& v2 = points_[e_second]; + CoalScalar d_six_vol = (v1.cross(v2)).dot(v3); + vol += d_six_vol; + } + } + + return vol / 6; +} + +template +void Convex::fillNeighbors() { + neighbors.reset(new std::vector(num_points)); + + typedef typename PolygonT::size_type size_type; + typedef typename PolygonT::index_type index_type; + std::vector> nneighbors(num_points); + unsigned int c_nneighbors = 0; + + if (!(polygons.get())) { + std::cerr << "Error in `Convex::fillNeighbors`! Convex has no polygons." + << std::endl; + } + const std::vector& polygons_ = *polygons; + for (unsigned int l = 0; l < num_polygons; ++l) { + const PolygonT& polygon = polygons_[l]; + const size_type n = polygon.size(); + + for (size_type j = 0; j < polygon.size(); ++j) { + size_type i = (j == 0) ? n - 1 : j - 1; + size_type k = (j == n - 1) ? 0 : j + 1; + index_type pi = polygon[(index_type)i], pj = polygon[(index_type)j], + pk = polygon[(index_type)k]; + // Update neighbors of pj; + if (nneighbors[pj].count(pi) == 0) { + c_nneighbors++; + nneighbors[pj].insert(pi); + } + if (nneighbors[pj].count(pk) == 0) { + c_nneighbors++; + nneighbors[pj].insert(pk); + } + } + } + + nneighbors_.reset(new std::vector(c_nneighbors)); + + unsigned int* p_nneighbors = nneighbors_->data(); + std::vector& neighbors_ = *neighbors; + for (unsigned int i = 0; i < num_points; ++i) { + Neighbors& n = neighbors_[i]; + if (nneighbors[i].size() >= (std::numeric_limits::max)()) + COAL_THROW_PRETTY("Too many neighbors.", std::logic_error); + n.count_ = (unsigned char)nneighbors[i].size(); + n.n_ = p_nneighbors; + p_nneighbors = + std::copy(nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors); + } + assert(p_nneighbors == nneighbors_->data() + c_nneighbors); +} + +} // namespace coal + +#endif diff --git a/include/coal/shape/geometric_shape_to_BVH_model.h b/include/coal/shape/geometric_shape_to_BVH_model.h new file mode 100644 index 000000000..6f43e9d7d --- /dev/null +++ b/include/coal/shape/geometric_shape_to_BVH_model.h @@ -0,0 +1,354 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H +#define COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H + +#include "coal/shape/geometric_shapes.h" +#include "coal/BVH/BVH_model.h" +#include + +namespace coal { + +/// @brief Generate BVH model from box +template +void generateBVHModel(BVHModel& model, const Box& shape, + const Transform3s& pose) { + CoalScalar a = shape.halfSide[0]; + CoalScalar b = shape.halfSide[1]; + CoalScalar c = shape.halfSide[2]; + std::vector points(8); + std::vector tri_indices(12); + points[0] = Vec3s(a, -b, c); + points[1] = Vec3s(a, b, c); + points[2] = Vec3s(-a, b, c); + points[3] = Vec3s(-a, -b, c); + points[4] = Vec3s(a, -b, -c); + points[5] = Vec3s(a, b, -c); + points[6] = Vec3s(-a, b, -c); + points[7] = Vec3s(-a, -b, -c); + + tri_indices[0].set(0, 4, 1); + tri_indices[1].set(1, 4, 5); + tri_indices[2].set(2, 6, 3); + tri_indices[3].set(3, 6, 7); + tri_indices[4].set(3, 0, 2); + tri_indices[5].set(2, 0, 1); + tri_indices[6].set(6, 5, 7); + tri_indices[7].set(7, 5, 4); + tri_indices[8].set(1, 5, 2); + tri_indices[9].set(2, 5, 6); + tri_indices[10].set(3, 7, 0); + tri_indices[11].set(0, 7, 4); + + for (unsigned int i = 0; i < points.size(); ++i) { + points[i] = pose.transform(points[i]).eval(); + } + + model.beginModel(); + model.addSubModel(points, tri_indices); + model.endModel(); + model.computeLocalAABB(); +} + +/// @brief Generate BVH model from sphere, given the number of segments along +/// longitude and number of rings along latitude. +template +void generateBVHModel(BVHModel& model, const Sphere& shape, + const Transform3s& pose, unsigned int seg, + unsigned int ring) { + std::vector points; + std::vector tri_indices; + + CoalScalar r = shape.radius; + CoalScalar phi, phid; + const CoalScalar pi = boost::math::constants::pi(); + phid = pi * 2 / seg; + phi = 0; + + CoalScalar theta, thetad; + thetad = pi / (ring + 1); + theta = 0; + + for (unsigned int i = 0; i < ring; ++i) { + CoalScalar theta_ = theta + thetad * (i + 1); + for (unsigned int j = 0; j < seg; ++j) { + points.push_back(Vec3s(r * sin(theta_) * cos(phi + j * phid), + r * sin(theta_) * sin(phi + j * phid), + r * cos(theta_))); + } + } + points.push_back(Vec3s(0, 0, r)); + points.push_back(Vec3s(0, 0, -r)); + + for (unsigned int i = 0; i < ring - 1; ++i) { + for (unsigned int j = 0; j < seg; ++j) { + unsigned int a, b, c, d; + a = i * seg + j; + b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1); + c = (i + 1) * seg + j; + d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1); + tri_indices.push_back(Triangle(a, c, b)); + tri_indices.push_back(Triangle(b, c, d)); + } + } + + for (unsigned int j = 0; j < seg; ++j) { + unsigned int a, b; + a = j; + b = (j == seg - 1) ? 0 : (j + 1); + tri_indices.push_back(Triangle(ring * seg, a, b)); + + a = (ring - 1) * seg + j; + b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1); + tri_indices.push_back(Triangle(a, ring * seg + 1, b)); + } + + for (unsigned int i = 0; i < points.size(); ++i) { + points[i] = pose.transform(points[i]); + } + + model.beginModel(); + model.addSubModel(points, tri_indices); + model.endModel(); + model.computeLocalAABB(); +} + +/// @brief Generate BVH model from sphere +/// The difference between generateBVHModel is that it gives the number of +/// triangles faces N for a sphere with unit radius. For sphere of radius r, +/// then the number of triangles is r * r * N so that the area represented by a +/// single triangle is approximately the same.s +template +void generateBVHModel(BVHModel& model, const Sphere& shape, + const Transform3s& pose, + unsigned int n_faces_for_unit_sphere) { + CoalScalar r = shape.radius; + CoalScalar n_low_bound = + std::sqrt((CoalScalar)n_faces_for_unit_sphere / CoalScalar(2.)) * r * r; + unsigned int ring = (unsigned int)ceil(n_low_bound); + unsigned int seg = (unsigned int)ceil(n_low_bound); + + generateBVHModel(model, shape, pose, seg, ring); +} + +/// @brief Generate BVH model from cylinder, given the number of segments along +/// circle and the number of segments along axis. +template +void generateBVHModel(BVHModel& model, const Cylinder& shape, + const Transform3s& pose, unsigned int tot, + unsigned int h_num) { + std::vector points; + std::vector tri_indices; + + CoalScalar r = shape.radius; + CoalScalar h = shape.halfLength; + CoalScalar phi, phid; + const CoalScalar pi = boost::math::constants::pi(); + phid = pi * 2 / tot; + phi = 0; + + CoalScalar hd = 2 * h / h_num; + + for (unsigned int i = 0; i < tot; ++i) + points.push_back( + Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), h)); + + for (unsigned int i = 0; i < h_num - 1; ++i) { + for (unsigned int j = 0; j < tot; ++j) { + points.push_back(Vec3s(r * cos(phi + phid * j), r * sin(phi + phid * j), + h - (i + 1) * hd)); + } + } + + for (unsigned int i = 0; i < tot; ++i) + points.push_back( + Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), -h)); + + points.push_back(Vec3s(0, 0, h)); + points.push_back(Vec3s(0, 0, -h)); + + for (unsigned int i = 0; i < tot; ++i) { + Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1))); + tri_indices.push_back(tmp); + } + + for (unsigned int i = 0; i < tot; ++i) { + Triangle tmp((h_num + 1) * tot + 1, + h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i); + tri_indices.push_back(tmp); + } + + for (unsigned int i = 0; i < h_num; ++i) { + for (unsigned int j = 0; j < tot; ++j) { + unsigned int a, b, c, d; + a = j; + b = (j == tot - 1) ? 0 : (j + 1); + c = j + tot; + d = (j == tot - 1) ? tot : (j + 1 + tot); + + unsigned int start = i * tot; + tri_indices.push_back(Triangle(start + b, start + a, start + c)); + tri_indices.push_back(Triangle(start + b, start + c, start + d)); + } + } + + for (unsigned int i = 0; i < points.size(); ++i) { + points[i] = pose.transform(points[i]); + } + + model.beginModel(); + model.addSubModel(points, tri_indices); + model.endModel(); + model.computeLocalAABB(); +} + +/// @brief Generate BVH model from cylinder +/// Difference from generateBVHModel: is that it gives the circle split number +/// tot for a cylinder with unit radius. For cylinder with larger radius, the +/// number of circle split number is r * tot. +template +void generateBVHModel(BVHModel& model, const Cylinder& shape, + const Transform3s& pose, + unsigned int tot_for_unit_cylinder) { + CoalScalar r = shape.radius; + CoalScalar h = 2 * shape.halfLength; + + const CoalScalar pi = boost::math::constants::pi(); + unsigned int tot = (unsigned int)(tot_for_unit_cylinder * r); + CoalScalar phid = pi * 2 / tot; + + CoalScalar circle_edge = phid * r; + unsigned int h_num = (unsigned int)ceil(h / circle_edge); + + generateBVHModel(model, shape, pose, tot, h_num); +} + +/// @brief Generate BVH model from cone, given the number of segments along +/// circle and the number of segments along axis. +template +void generateBVHModel(BVHModel& model, const Cone& shape, + const Transform3s& pose, unsigned int tot, + unsigned int h_num) { + std::vector points; + std::vector tri_indices; + + CoalScalar r = shape.radius; + CoalScalar h = shape.halfLength; + + CoalScalar phi, phid; + const CoalScalar pi = boost::math::constants::pi(); + phid = pi * 2 / tot; + phi = 0; + + CoalScalar hd = 2 * h / h_num; + + for (unsigned int i = 0; i < h_num - 1; ++i) { + CoalScalar h_i = h - (i + 1) * hd; + CoalScalar rh = r * (0.5 - h_i / h / 2); + for (unsigned int j = 0; j < tot; ++j) { + points.push_back( + Vec3s(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i)); + } + } + + for (unsigned int i = 0; i < tot; ++i) + points.push_back( + Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), -h)); + + points.push_back(Vec3s(0, 0, h)); + points.push_back(Vec3s(0, 0, -h)); + + for (unsigned int i = 0; i < tot; ++i) { + Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1)); + tri_indices.push_back(tmp); + } + + for (unsigned int i = 0; i < tot; ++i) { + Triangle tmp(h_num * tot + 1, + (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), + (h_num - 1) * tot + i); + tri_indices.push_back(tmp); + } + + for (unsigned int i = 0; i < h_num - 1; ++i) { + for (unsigned int j = 0; j < tot; ++j) { + unsigned int a, b, c, d; + a = j; + b = (j == tot - 1) ? 0 : (j + 1); + c = j + tot; + d = (j == tot - 1) ? tot : (j + 1 + tot); + + unsigned int start = i * tot; + tri_indices.push_back(Triangle(start + b, start + a, start + c)); + tri_indices.push_back(Triangle(start + b, start + c, start + d)); + } + } + + for (unsigned int i = 0; i < points.size(); ++i) { + points[i] = pose.transform(points[i]); + } + + model.beginModel(); + model.addSubModel(points, tri_indices); + model.endModel(); + model.computeLocalAABB(); +} + +/// @brief Generate BVH model from cone +/// Difference from generateBVHModel: is that it gives the circle split number +/// tot for a cylinder with unit radius. For cone with larger radius, the number +/// of circle split number is r * tot. +template +void generateBVHModel(BVHModel& model, const Cone& shape, + const Transform3s& pose, unsigned int tot_for_unit_cone) { + CoalScalar r = shape.radius; + CoalScalar h = 2 * shape.halfLength; + + const CoalScalar pi = boost::math::constants::pi(); + unsigned int tot = (unsigned int)(tot_for_unit_cone * r); + CoalScalar phid = pi * 2 / tot; + + CoalScalar circle_edge = phid * r; + unsigned int h_num = (unsigned int)ceil(h / circle_edge); + + generateBVHModel(model, shape, pose, tot, h_num); +} + +} // namespace coal + +#endif diff --git a/include/coal/shape/geometric_shapes.h b/include/coal/shape/geometric_shapes.h new file mode 100644 index 000000000..1050f692a --- /dev/null +++ b/include/coal/shape/geometric_shapes.h @@ -0,0 +1,1059 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_GEOMETRIC_SHAPES_H +#define COAL_GEOMETRIC_SHAPES_H + +#include +#include + +#include + +#include "coal/collision_object.h" +#include "coal/data_types.h" + +#ifdef COAL_HAS_QHULL +namespace orgQhull { +class Qhull; +} +#endif + +namespace coal { + +/// @brief Base class for all basic geometric shapes +class COAL_DLLAPI ShapeBase : public CollisionGeometry { + public: + ShapeBase() {} + + ///  \brief Copy constructor + ShapeBase(const ShapeBase& other) + : CollisionGeometry(other), + m_swept_sphere_radius(other.m_swept_sphere_radius) {} + + ShapeBase& operator=(const ShapeBase& other) = default; + + virtual ~ShapeBase() {}; + + /// @brief Get object type: a geometric shape + OBJECT_TYPE getObjectType() const { return OT_GEOM; } + + /// @brief Set radius of sphere swept around the shape. + /// Must be >= 0. + void setSweptSphereRadius(CoalScalar radius) { + if (radius < 0) { + COAL_THROW_PRETTY("Swept-sphere radius must be positive.", + std::invalid_argument); + } + this->m_swept_sphere_radius = radius; + } + + /// @brief Get radius of sphere swept around the shape. + /// This radius is always >= 0. + CoalScalar getSweptSphereRadius() const { + return this->m_swept_sphere_radius; + } + + protected: + /// \brief Radius of the sphere swept around the shape. + /// Default value is 0. + /// Note: this property differs from `inflated` method of certain + /// derived classes (e.g. Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder) + /// in the sense that inflated returns a new shape which can be inflated but + /// also deflated. + /// Also, an inflated shape is not rounded. It simply has a different size. + /// Sweeping a shape with a sphere is a different operation (a Minkowski sum), + /// which rounds the sharp corners of a shape. + /// The swept sphere radius is a property of the shape itself and can be + /// manually updated between collision checks. + CoalScalar m_swept_sphere_radius{0}; +}; + +/// @defgroup Geometric_Shapes Geometric shapes +/// Classes of different types of geometric shapes. +/// @{ + +/// @brief Triangle stores the points instead of only indices of points +class COAL_DLLAPI TriangleP : public ShapeBase { + public: + TriangleP() {}; + + TriangleP(const Vec3s& a_, const Vec3s& b_, const Vec3s& c_) + : ShapeBase(), a(a_), b(b_), c(c_) {} + + TriangleP(const TriangleP& other) + : ShapeBase(other), a(other.a), b(other.b), c(other.c) {} + + /// @brief Clone *this into a new TriangleP + virtual TriangleP* clone() const { return new TriangleP(*this); }; + + /// @brief virtual function of compute AABB in local coordinate + void computeLocalAABB(); + + NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; } + + // std::pair inflated(const CoalScalar value) const + // { + // if (value == 0) return std::make_pair(new TriangleP(*this), + // Transform3s()); Vec3s AB(b - a), BC(c - b), CA(a - c); AB.normalize(); + // BC.normalize(); + // CA.normalize(); + // + // Vec3s new_a(a + value * Vec3s(-AB + CA).normalized()); + // Vec3s new_b(b + value * Vec3s(-BC + AB).normalized()); + // Vec3s new_c(c + value * Vec3s(-CA + BC).normalized()); + // + // return std::make_pair(new TriangleP(new_a, new_b, new_c), + // Transform3s()); + // } + // + // CoalScalar minInflationValue() const + // { + // return (std::numeric_limits::max)(); // TODO(jcarpent): + // implement + // } + + Vec3s a, b, c; + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const TriangleP* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const TriangleP& other = *other_ptr; + + return a == other.a && b == other.b && c == other.c && + getSweptSphereRadius() == other.getSweptSphereRadius(); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// @brief Center at zero point, axis aligned box +class COAL_DLLAPI Box : public ShapeBase { + public: + Box(CoalScalar x, CoalScalar y, CoalScalar z) + : ShapeBase(), halfSide(x / 2, y / 2, z / 2) {} + + Box(const Vec3s& side_) : ShapeBase(), halfSide(side_ / 2) {} + + Box(const Box& other) : ShapeBase(other), halfSide(other.halfSide) {} + + Box& operator=(const Box& other) { + if (this == &other) return *this; + + this->halfSide = other.halfSide; + return *this; + } + + /// @brief Clone *this into a new Box + virtual Box* clone() const { return new Box(*this); }; + + /// @brief Default constructor + Box() {} + + /// @brief box side half-length + Vec3s halfSide; + + /// @brief Compute AABB + void computeLocalAABB(); + + /// @brief Get node type: a box + NODE_TYPE getNodeType() const { return GEOM_BOX; } + + CoalScalar computeVolume() const { return 8 * halfSide.prod(); } + + Matrix3s computeMomentofInertia() const { + CoalScalar V = computeVolume(); + Vec3s s(halfSide.cwiseAbs2() * V); + return (Vec3s(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal(); + } + + CoalScalar minInflationValue() const { return -halfSide.minCoeff(); } + + /// \brief Inflate the box by an amount given by `value`. + /// This value can be positive or negative but must always >= + /// `minInflationValue()`. + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated box and the related transform to account for the + /// change of shape frame + std::pair inflated(const CoalScalar value) const { + if (value <= minInflationValue()) + COAL_THROW_PRETTY("value (" << value << ") " + << "is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); + return std::make_pair(Box(2 * (halfSide + Vec3s::Constant(value))), + Transform3s()); + } + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const Box* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const Box& other = *other_ptr; + + return halfSide == other.halfSide && + getSweptSphereRadius() == other.getSweptSphereRadius(); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// @brief Center at zero point sphere +class COAL_DLLAPI Sphere : public ShapeBase { + public: + /// @brief Default constructor + Sphere() {} + + explicit Sphere(CoalScalar radius_) : ShapeBase(), radius(radius_) {} + + Sphere(const Sphere& other) : ShapeBase(other), radius(other.radius) {} + + /// @brief Clone *this into a new Sphere + virtual Sphere* clone() const { return new Sphere(*this); }; + + /// @brief Radius of the sphere + CoalScalar radius; + + /// @brief Compute AABB + void computeLocalAABB(); + + /// @brief Get node type: a sphere + NODE_TYPE getNodeType() const { return GEOM_SPHERE; } + + Matrix3s computeMomentofInertia() const { + CoalScalar I = 0.4 * radius * radius * computeVolume(); + return I * Matrix3s::Identity(); + } + + CoalScalar computeVolume() const { + return 4 * boost::math::constants::pi() * radius * radius * + radius / 3; + } + + CoalScalar minInflationValue() const { return -radius; } + + /// \brief Inflate the sphere by an amount given by `value`. + /// This value can be positive or negative but must always >= + /// `minInflationValue()`. + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated sphere and the related transform to account for + /// the change of shape frame + std::pair inflated(const CoalScalar value) const { + if (value <= minInflationValue()) + COAL_THROW_PRETTY("value (" << value + << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); + return std::make_pair(Sphere(radius + value), Transform3s()); + } + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const Sphere* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const Sphere& other = *other_ptr; + + return radius == other.radius && + getSweptSphereRadius() == other.getSweptSphereRadius(); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// @brief Ellipsoid centered at point zero +class COAL_DLLAPI Ellipsoid : public ShapeBase { + public: + /// @brief Default constructor + Ellipsoid() {} + + Ellipsoid(CoalScalar rx, CoalScalar ry, CoalScalar rz) + : ShapeBase(), radii(rx, ry, rz) {} + + explicit Ellipsoid(const Vec3s& radii) : radii(radii) {} + + Ellipsoid(const Ellipsoid& other) : ShapeBase(other), radii(other.radii) {} + + /// @brief Clone *this into a new Ellipsoid + virtual Ellipsoid* clone() const { return new Ellipsoid(*this); }; + + /// @brief Radii of the Ellipsoid (such that on boundary: x^2/rx^2 + y^2/ry^2 + /// + z^2/rz^2 = 1) + Vec3s radii; + + /// @brief Compute AABB + void computeLocalAABB(); + + /// @brief Get node type: an ellipsoid + NODE_TYPE getNodeType() const { return GEOM_ELLIPSOID; } + + Matrix3s computeMomentofInertia() const { + CoalScalar V = computeVolume(); + CoalScalar a2 = V * radii[0] * radii[0]; + CoalScalar b2 = V * radii[1] * radii[1]; + CoalScalar c2 = V * radii[2] * radii[2]; + return (Matrix3s() << 0.2 * (b2 + c2), 0, 0, 0, 0.2 * (a2 + c2), 0, 0, 0, + 0.2 * (a2 + b2)) + .finished(); + } + + CoalScalar computeVolume() const { + return 4 * boost::math::constants::pi() * radii[0] * radii[1] * + radii[2] / 3; + } + + CoalScalar minInflationValue() const { return -radii.minCoeff(); } + + /// \brief Inflate the ellipsoid by an amount given by `value`. + /// This value can be positive or negative but must always >= + /// `minInflationValue()`. + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated ellipsoid and the related transform to account for + /// the change of shape frame + std::pair inflated(const CoalScalar value) const { + if (value <= minInflationValue()) + COAL_THROW_PRETTY("value (" << value + << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); + return std::make_pair(Ellipsoid(radii + Vec3s::Constant(value)), + Transform3s()); + } + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const Ellipsoid* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const Ellipsoid& other = *other_ptr; + + return radii == other.radii && + getSweptSphereRadius() == other.getSweptSphereRadius(); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// @brief Capsule +/// It is \f$ { x~\in~\mathbb{R}^3, d(x, AB) \leq radius } \f$ +/// where \f$ d(x, AB) \f$ is the distance between the point x and the capsule +/// segment AB, with \f$ A = (0,0,-halfLength), B = (0,0,halfLength) \f$. +class COAL_DLLAPI Capsule : public ShapeBase { + public: + /// @brief Default constructor + Capsule() {} + + Capsule(CoalScalar radius_, CoalScalar lz_) : ShapeBase(), radius(radius_) { + halfLength = lz_ / 2; + } + + Capsule(const Capsule& other) + : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {} + + /// @brief Clone *this into a new Capsule + virtual Capsule* clone() const { return new Capsule(*this); }; + + /// @brief Radius of capsule + CoalScalar radius; + + /// @brief Half Length along z axis + CoalScalar halfLength; + + /// @brief Compute AABB + void computeLocalAABB(); + + /// @brief Get node type: a capsule + NODE_TYPE getNodeType() const { return GEOM_CAPSULE; } + + CoalScalar computeVolume() const { + return boost::math::constants::pi() * radius * radius * + ((halfLength * 2) + radius * 4 / 3.0); + } + + Matrix3s computeMomentofInertia() const { + CoalScalar v_cyl = radius * radius * (halfLength * 2) * + boost::math::constants::pi(); + CoalScalar v_sph = radius * radius * radius * + boost::math::constants::pi() * 4 / 3.0; + + CoalScalar h2 = halfLength * halfLength; + CoalScalar r2 = radius * radius; + CoalScalar ix = v_cyl * (h2 / 3. + r2 / 4.) + + v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength); + CoalScalar iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius; + + return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); + } + + CoalScalar minInflationValue() const { return -radius; } + + /// \brief Inflate the capsule by an amount given by `value`. + /// This value can be positive or negative but must always >= + /// `minInflationValue()`. + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated capsule and the related transform to account for + /// the change of shape frame + std::pair inflated(const CoalScalar value) const { + if (value <= minInflationValue()) + COAL_THROW_PRETTY("value (" << value + << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); + return std::make_pair(Capsule(radius + value, 2 * halfLength), + Transform3s()); + } + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const Capsule* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const Capsule& other = *other_ptr; + + return radius == other.radius && halfLength == other.halfLength && + getSweptSphereRadius() == other.getSweptSphereRadius(); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// @brief Cone +/// The base of the cone is at \f$ z = - halfLength \f$ and the top is at +/// \f$ z = halfLength \f$. +class COAL_DLLAPI Cone : public ShapeBase { + public: + /// @brief Default constructor + Cone() {} + + Cone(CoalScalar radius_, CoalScalar lz_) : ShapeBase(), radius(radius_) { + halfLength = lz_ / 2; + } + + Cone(const Cone& other) + : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {} + + /// @brief Clone *this into a new Cone + virtual Cone* clone() const { return new Cone(*this); }; + + /// @brief Radius of the cone + CoalScalar radius; + + /// @brief Half Length along z axis + CoalScalar halfLength; + + /// @brief Compute AABB + void computeLocalAABB(); + + /// @brief Get node type: a cone + NODE_TYPE getNodeType() const { return GEOM_CONE; } + + CoalScalar computeVolume() const { + return boost::math::constants::pi() * radius * radius * + (halfLength * 2) / 3; + } + + Matrix3s computeMomentofInertia() const { + CoalScalar V = computeVolume(); + CoalScalar ix = + V * (0.4 * halfLength * halfLength + 3 * radius * radius / 20); + CoalScalar iz = 0.3 * V * radius * radius; + + return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); + } + + Vec3s computeCOM() const { return Vec3s(0, 0, -0.5 * halfLength); } + + CoalScalar minInflationValue() const { + return -(std::min)(radius, halfLength); + } + + /// \brief Inflate the cone by an amount given by `value`. + /// This value can be positive or negative but must always >= + /// `minInflationValue()`. + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated cone and the related transform to account for the + /// change of shape frame + std::pair inflated(const CoalScalar value) const { + if (value <= minInflationValue()) + COAL_THROW_PRETTY("value (" << value + << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); + + // tan(alpha) = 2*halfLength/radius; + const CoalScalar tan_alpha = 2 * halfLength / radius; + const CoalScalar sin_alpha = + tan_alpha / std::sqrt(1 + tan_alpha * tan_alpha); + const CoalScalar top_inflation = value / sin_alpha; + const CoalScalar bottom_inflation = value; + + const CoalScalar new_lz = 2 * halfLength + top_inflation + bottom_inflation; + const CoalScalar new_cz = (top_inflation + bottom_inflation) / 2.; + const CoalScalar new_radius = new_lz / tan_alpha; + + return std::make_pair(Cone(new_radius, new_lz), + Transform3s(Vec3s(0., 0., new_cz))); + } + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const Cone* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const Cone& other = *other_ptr; + + return radius == other.radius && halfLength == other.halfLength && + getSweptSphereRadius() == other.getSweptSphereRadius(); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// @brief Cylinder along Z axis. +/// The cylinder is defined at its centroid. +class COAL_DLLAPI Cylinder : public ShapeBase { + public: + /// @brief Default constructor + Cylinder() {} + + Cylinder(CoalScalar radius_, CoalScalar lz_) : ShapeBase(), radius(radius_) { + halfLength = lz_ / 2; + } + + Cylinder(const Cylinder& other) + : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {} + + Cylinder& operator=(const Cylinder& other) { + if (this == &other) return *this; + + this->radius = other.radius; + this->halfLength = other.halfLength; + return *this; + } + + /// @brief Clone *this into a new Cylinder + virtual Cylinder* clone() const { return new Cylinder(*this); }; + + /// @brief Radius of the cylinder + CoalScalar radius; + + /// @brief Half Length along z axis + CoalScalar halfLength; + + /// @brief Compute AABB + void computeLocalAABB(); + + /// @brief Get node type: a cylinder + NODE_TYPE getNodeType() const { return GEOM_CYLINDER; } + + CoalScalar computeVolume() const { + return boost::math::constants::pi() * radius * radius * + (halfLength * 2); + } + + Matrix3s computeMomentofInertia() const { + CoalScalar V = computeVolume(); + CoalScalar ix = V * (radius * radius / 4 + halfLength * halfLength / 3); + CoalScalar iz = V * radius * radius / 2; + return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); + } + + CoalScalar minInflationValue() const { + return -(std::min)(radius, halfLength); + } + + /// \brief Inflate the cylinder by an amount given by `value`. + /// This value can be positive or negative but must always >= + /// `minInflationValue()`. + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated cylinder and the related transform to account for + /// the change of shape frame + std::pair inflated(const CoalScalar value) const { + if (value <= minInflationValue()) + COAL_THROW_PRETTY("value (" << value + << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); + return std::make_pair(Cylinder(radius + value, 2 * (halfLength + value)), + Transform3s()); + } + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const Cylinder* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const Cylinder& other = *other_ptr; + + return radius == other.radius && halfLength == other.halfLength && + getSweptSphereRadius() == other.getSweptSphereRadius(); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// @brief Base for convex polytope. +/// @note Inherited classes are responsible for filling ConvexBase::neighbors; +class COAL_DLLAPI ConvexBase : public ShapeBase { + public: + /// @brief Build a convex hull based on Qhull library + /// and store the vertices and optionally the triangles + /// \param points, num_points the points whose convex hull should be computed. + /// \param keepTriangles if \c true, returns a Convex object which + /// contains the triangle of the shape. + /// \param qhullCommand the command sent to qhull. + /// - if \c keepTriangles is \c true, this parameter should include + /// "Qt". If \c NULL, "Qt" is passed to Qhull. + /// - if \c keepTriangles is \c false, an empty string is passed to + /// Qhull. + /// \note Coal must have been compiled with option \c COAL_HAS_QHULL set + /// to \c ON. + static ConvexBase* convexHull(std::shared_ptr>& points, + unsigned int num_points, bool keepTriangles, + const char* qhullCommand = NULL); + + // TODO(louis): put this method in private sometime in the future. + COAL_DEPRECATED static ConvexBase* convexHull( + const Vec3s* points, unsigned int num_points, bool keepTriangles, + const char* qhullCommand = NULL); + + virtual ~ConvexBase(); + + /// @brief Clone (deep copy). + /// This method is consistent with BVHModel `clone` method. + /// The copy constructor is called, which duplicates the data. + virtual ConvexBase* clone() const { return new ConvexBase(*this); } + + /// @brief Compute AABB + void computeLocalAABB(); + + /// @brief Get node type: a convex polytope + NODE_TYPE getNodeType() const { return GEOM_CONVEX; } + +#ifdef COAL_HAS_QHULL + /// @brief Builds the double description of the convex polytope, i.e. the set + /// of hyperplanes which intersection form the polytope. + void buildDoubleDescription(); +#endif + + struct COAL_DLLAPI Neighbors { + unsigned char count_; + unsigned int* n_; + + unsigned char const& count() const { return count_; } + unsigned int& operator[](int i) { + assert(i < count_); + return n_[i]; + } + unsigned int const& operator[](int i) const { + assert(i < count_); + return n_[i]; + } + + bool operator==(const Neighbors& other) const { + if (count_ != other.count_) return false; + + for (int i = 0; i < count_; ++i) { + if (n_[i] != other.n_[i]) return false; + } + + return true; + } + + bool operator!=(const Neighbors& other) const { return !(*this == other); } + }; + + /// @brief Above this threshold, the convex polytope is considered large. + /// This influcences the way the support function is computed. + static constexpr size_t num_vertices_large_convex_threshold = 32; + + /// @brief An array of the points of the polygon. + std::shared_ptr> points; + unsigned int num_points; + + /// @brief An array of the normals of the polygon. + std::shared_ptr> normals; + /// @brief An array of the offsets to the normals of the polygon. + /// Note: there are as many offsets as normals. + std::shared_ptr> offsets; + unsigned int num_normals_and_offsets; + + /// @brief Neighbors of each vertex. + /// It is an array of size num_points. For each vertex, it contains the number + /// of neighbors and a list of indices pointing to them. + std::shared_ptr> neighbors; + + /// @brief center of the convex polytope, this is used for collision: center + /// is guaranteed in the internal of the polytope (as it is convex) + Vec3s center; + + /// @brief The support warm start polytope contains certain points of `this` + /// which are support points in specific directions of space. + /// This struct is used to warm start the support function computation for + /// large meshes (`num_points` > 32). + struct SupportWarmStartPolytope { + /// @brief Array of support points to warm start the support function + /// computation. + std::vector points; + + /// @brief Indices of the support points warm starts. + /// These are the indices of the real convex, not the indices of points in + /// the warm start polytope. + std::vector indices; + }; + + /// @brief Number of support warm starts. + static constexpr size_t num_support_warm_starts = 14; + + /// @brief Support warm start polytopes. + SupportWarmStartPolytope support_warm_starts; + + protected: + /// @brief Construct an uninitialized convex object + /// Initialization is done with ConvexBase::initialize. + ConvexBase() + : ShapeBase(), + num_points(0), + num_normals_and_offsets(0), + center(Vec3s::Zero()) {} + + /// @brief Initialize the points of the convex shape + /// This also initializes the ConvexBase::center. + /// + /// \param ownStorage weither the ConvexBase owns the data. + /// \param points_ list of 3D points /// + /// \param num_points_ number of 3D points + void initialize(std::shared_ptr> points_, + unsigned int num_points_); + + /// @brief Set the points of the convex shape. + /// + /// \param ownStorage weither the ConvexBase owns the data. + /// \param points_ list of 3D points /// + /// \param num_points_ number of 3D points + void set(std::shared_ptr> points_, + unsigned int num_points_); + + /// @brief Copy constructor + /// Only the list of neighbors is copied. + ConvexBase(const ConvexBase& other); + +#ifdef COAL_HAS_QHULL + void buildDoubleDescriptionFromQHullResult(const orgQhull::Qhull& qh); +#endif + + /// @brief Build the support points warm starts. + void buildSupportWarmStart(); + + /// @brief Array of indices of the neighbors of each vertex. + /// Since we don't know a priori the number of neighbors of each vertex, we + /// store the indices of the neighbors in a single array. + /// The `neighbors` attribute, an array of `Neighbors`, is used to point each + /// vertex to the right indices in the `nneighbors_` array. + std::shared_ptr> nneighbors_; + + private: + void computeCenter(); + + virtual bool isEqual(const CollisionGeometry& _other) const { + const ConvexBase* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const ConvexBase& other = *other_ptr; + + if (num_points != other.num_points) return false; + + if ((!(points.get()) && other.points.get()) || + (points.get() && !(other.points.get()))) + return false; + if (points.get() && other.points.get()) { + const std::vector& points_ = *points; + const std::vector& other_points_ = *(other.points); + for (unsigned int i = 0; i < num_points; ++i) { + if (points_[i] != (other_points_)[i]) return false; + } + } + + if ((!(neighbors.get()) && other.neighbors.get()) || + (neighbors.get() && !(other.neighbors.get()))) + return false; + if (neighbors.get() && other.neighbors.get()) { + const std::vector& neighbors_ = *neighbors; + const std::vector& other_neighbors_ = *(other.neighbors); + for (unsigned int i = 0; i < num_points; ++i) { + if (neighbors_[i] != other_neighbors_[i]) return false; + } + } + + if ((!(normals.get()) && other.normals.get()) || + (normals.get() && !(other.normals.get()))) + return false; + if (normals.get() && other.normals.get()) { + const std::vector& normals_ = *normals; + const std::vector& other_normals_ = *(other.normals); + for (unsigned int i = 0; i < num_normals_and_offsets; ++i) { + if (normals_[i] != other_normals_[i]) return false; + } + } + + if ((!(offsets.get()) && other.offsets.get()) || + (offsets.get() && !(other.offsets.get()))) + return false; + if (offsets.get() && other.offsets.get()) { + const std::vector& offsets_ = *offsets; + const std::vector& other_offsets_ = *(other.offsets); + for (unsigned int i = 0; i < num_normals_and_offsets; ++i) { + if (offsets_[i] != other_offsets_[i]) return false; + } + } + + if (this->support_warm_starts.points.size() != + other.support_warm_starts.points.size() || + this->support_warm_starts.indices.size() != + other.support_warm_starts.indices.size()) { + return false; + } + + for (size_t i = 0; i < this->support_warm_starts.points.size(); ++i) { + if (this->support_warm_starts.points[i] != + other.support_warm_starts.points[i] || + this->support_warm_starts.indices[i] != + other.support_warm_starts.indices[i]) { + return false; + } + } + + return center == other.center && + getSweptSphereRadius() == other.getSweptSphereRadius(); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +template +class Convex; + +/// @brief Half Space: this is equivalent to the Plane in ODE. +/// A Half space has a priviledged direction: the direction of the normal. +/// The separation plane is defined as n * x = d; Points in the negative side of +/// the separation plane (i.e. {x | n * x < d}) are inside the half space and +/// points in the positive side of the separation plane (i.e. {x | n * x > d}) +/// are outside the half space. +/// Note: prefer using a Halfspace instead of a Plane if possible, it has better +/// behavior w.r.t. collision detection algorithms. +class COAL_DLLAPI Halfspace : public ShapeBase { + public: + /// @brief Construct a half space with normal direction and offset + Halfspace(const Vec3s& n_, CoalScalar d_) : ShapeBase(), n(n_), d(d_) { + unitNormalTest(); + } + + /// @brief Construct a plane with normal direction and offset + Halfspace(CoalScalar a, CoalScalar b, CoalScalar c, CoalScalar d_) + : ShapeBase(), n(a, b, c), d(d_) { + unitNormalTest(); + } + + Halfspace() : ShapeBase(), n(1, 0, 0), d(0) {} + + Halfspace(const Halfspace& other) + : ShapeBase(other), n(other.n), d(other.d) {} + + /// @brief operator = + Halfspace& operator=(const Halfspace& other) { + n = other.n; + d = other.d; + return *this; + } + + /// @brief Clone *this into a new Halfspace + virtual Halfspace* clone() const { return new Halfspace(*this); }; + + CoalScalar signedDistance(const Vec3s& p) const { + return n.dot(p) - (d + this->getSweptSphereRadius()); + } + + CoalScalar distance(const Vec3s& p) const { + return std::abs(this->signedDistance(p)); + } + + /// @brief Compute AABB + void computeLocalAABB(); + + /// @brief Get node type: a half space + NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; } + + CoalScalar minInflationValue() const { + return std::numeric_limits::lowest(); + } + + /// \brief Inflate the halfspace by an amount given by `value`. + /// This value can be positive or negative but must always >= + /// `minInflationValue()`. + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated halfspace and the related transform to account for + /// the change of shape frame + std::pair inflated(const CoalScalar value) const { + if (value <= minInflationValue()) + COAL_THROW_PRETTY("value (" << value + << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); + return std::make_pair(Halfspace(n, d + value), Transform3s()); + } + + /// @brief Plane normal + Vec3s n; + + /// @brief Plane offset + CoalScalar d; + + protected: + /// @brief Turn non-unit normal into unit + void unitNormalTest(); + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const Halfspace* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const Halfspace& other = *other_ptr; + + return n == other.n && d == other.d && + getSweptSphereRadius() == other.getSweptSphereRadius(); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// @brief Infinite plane. +/// A plane can be viewed as two half spaces; it has no priviledged direction. +/// Note: prefer using a Halfspace instead of a Plane if possible, it has better +/// behavior w.r.t. collision detection algorithms. +class COAL_DLLAPI Plane : public ShapeBase { + public: + /// @brief Construct a plane with normal direction and offset + Plane(const Vec3s& n_, CoalScalar d_) : ShapeBase(), n(n_), d(d_) { + unitNormalTest(); + } + + /// @brief Construct a plane with normal direction and offset + Plane(CoalScalar a, CoalScalar b, CoalScalar c, CoalScalar d_) + : ShapeBase(), n(a, b, c), d(d_) { + unitNormalTest(); + } + + Plane() : ShapeBase(), n(1, 0, 0), d(0) {} + + Plane(const Plane& other) : ShapeBase(other), n(other.n), d(other.d) {} + + /// @brief operator = + Plane& operator=(const Plane& other) { + n = other.n; + d = other.d; + return *this; + } + + /// @brief Clone *this into a new Plane + virtual Plane* clone() const { return new Plane(*this); }; + + CoalScalar signedDistance(const Vec3s& p) const { + const CoalScalar dist = n.dot(p) - d; + CoalScalar signed_dist = + std::abs(n.dot(p) - d) - this->getSweptSphereRadius(); + if (dist >= 0) { + return signed_dist; + } + if (signed_dist >= 0) { + return -signed_dist; + } + return signed_dist; + } + + CoalScalar distance(const Vec3s& p) const { + return std::abs(std::abs(n.dot(p) - d) - this->getSweptSphereRadius()); + } + + /// @brief Compute AABB + void computeLocalAABB(); + + /// @brief Get node type: a plane + NODE_TYPE getNodeType() const { return GEOM_PLANE; } + + /// @brief Plane normal + Vec3s n; + + /// @brief Plane offset + CoalScalar d; + + protected: + /// @brief Turn non-unit normal into unit + void unitNormalTest(); + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const Plane* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const Plane& other = *other_ptr; + + return n == other.n && d == other.d && + getSweptSphereRadius() == other.getSweptSphereRadius(); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace coal + +#endif diff --git a/include/coal/shape/geometric_shapes_traits.h b/include/coal/shape/geometric_shapes_traits.h new file mode 100644 index 000000000..fcc769e66 --- /dev/null +++ b/include/coal/shape/geometric_shapes_traits.h @@ -0,0 +1,158 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2015-2022, CNRS, Inria + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef COAL_GEOMETRIC_SHAPES_TRAITS_H +#define COAL_GEOMETRIC_SHAPES_TRAITS_H + +#include "coal/shape/geometric_shapes.h" + +namespace coal { + +struct shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = false, + HasInflatedSupportFunction = false, + IsStrictlyConvex = false + }; +}; + +template +struct shape_traits : shape_traits_base {}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false, + IsStrictlyConvex = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false, + IsStrictlyConvex = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false, + IsStrictlyConvex = true + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false, + IsStrictlyConvex = true + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false, + IsStrictlyConvex = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false, + IsStrictlyConvex = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false, + IsStrictlyConvex = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = true, + IsInflatable = true, + HasInflatedSupportFunction = true, + IsStrictlyConvex = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false, + IsStrictlyConvex = false + }; +}; + +} // namespace coal + +#endif // ifndef COAL_GEOMETRIC_SHAPES_TRAITS_H diff --git a/include/coal/shape/geometric_shapes_utility.h b/include/coal/shape/geometric_shapes_utility.h new file mode 100644 index 000000000..76ac78ae8 --- /dev/null +++ b/include/coal/shape/geometric_shapes_utility.h @@ -0,0 +1,261 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_GEOMETRIC_SHAPES_UTILITY_H +#define COAL_GEOMETRIC_SHAPES_UTILITY_H + +#include +#include "coal/shape/geometric_shapes.h" +#include "coal/BV/BV.h" +#include "coal/internal/BV_fitter.h" + +namespace coal { + +/// @cond IGNORE +namespace details { +/// @brief get the vertices of some convex shape which can bound the given shape +/// in a specific configuration +COAL_DLLAPI std::vector getBoundVertices(const Box& box, + const Transform3s& tf); +COAL_DLLAPI std::vector getBoundVertices(const Sphere& sphere, + const Transform3s& tf); +COAL_DLLAPI std::vector getBoundVertices(const Ellipsoid& ellipsoid, + const Transform3s& tf); +COAL_DLLAPI std::vector getBoundVertices(const Capsule& capsule, + const Transform3s& tf); +COAL_DLLAPI std::vector getBoundVertices(const Cone& cone, + const Transform3s& tf); +COAL_DLLAPI std::vector getBoundVertices(const Cylinder& cylinder, + const Transform3s& tf); +COAL_DLLAPI std::vector getBoundVertices(const ConvexBase& convex, + const Transform3s& tf); +COAL_DLLAPI std::vector getBoundVertices(const TriangleP& triangle, + const Transform3s& tf); +} // namespace details +/// @endcond + +/// @brief calculate a bounding volume for a shape in a specific configuration +template +inline void computeBV(const S& s, const Transform3s& tf, BV& bv) { + if (s.getSweptSphereRadius() > 0) { + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); + } + std::vector convex_bound_vertices = details::getBoundVertices(s, tf); + fit(&convex_bound_vertices[0], (unsigned int)convex_bound_vertices.size(), + bv); +} + +template <> +COAL_DLLAPI void computeBV(const Box& s, const Transform3s& tf, + AABB& bv); + +template <> +COAL_DLLAPI void computeBV(const Sphere& s, const Transform3s& tf, + AABB& bv); + +template <> +COAL_DLLAPI void computeBV(const Ellipsoid& e, + const Transform3s& tf, AABB& bv); + +template <> +COAL_DLLAPI void computeBV(const Capsule& s, + const Transform3s& tf, AABB& bv); + +template <> +COAL_DLLAPI void computeBV(const Cone& s, const Transform3s& tf, + AABB& bv); + +template <> +COAL_DLLAPI void computeBV(const Cylinder& s, + const Transform3s& tf, AABB& bv); + +template <> +COAL_DLLAPI void computeBV(const ConvexBase& s, + const Transform3s& tf, AABB& bv); + +template <> +COAL_DLLAPI void computeBV(const TriangleP& s, + const Transform3s& tf, AABB& bv); + +template <> +COAL_DLLAPI void computeBV(const Halfspace& s, + const Transform3s& tf, AABB& bv); + +template <> +COAL_DLLAPI void computeBV(const Plane& s, const Transform3s& tf, + AABB& bv); + +template <> +COAL_DLLAPI void computeBV(const Box& s, const Transform3s& tf, + OBB& bv); + +template <> +COAL_DLLAPI void computeBV(const Sphere& s, const Transform3s& tf, + OBB& bv); + +template <> +COAL_DLLAPI void computeBV(const Capsule& s, + const Transform3s& tf, OBB& bv); + +template <> +COAL_DLLAPI void computeBV(const Cone& s, const Transform3s& tf, + OBB& bv); + +template <> +COAL_DLLAPI void computeBV(const Cylinder& s, + const Transform3s& tf, OBB& bv); + +template <> +COAL_DLLAPI void computeBV(const ConvexBase& s, + const Transform3s& tf, OBB& bv); + +template <> +COAL_DLLAPI void computeBV(const Halfspace& s, + const Transform3s& tf, OBB& bv); + +template <> +COAL_DLLAPI void computeBV(const Halfspace& s, + const Transform3s& tf, RSS& bv); + +template <> +COAL_DLLAPI void computeBV(const Halfspace& s, + const Transform3s& tf, + OBBRSS& bv); + +template <> +COAL_DLLAPI void computeBV(const Halfspace& s, + const Transform3s& tf, kIOS& bv); + +template <> +COAL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, + const Transform3s& tf, + KDOP<16>& bv); + +template <> +COAL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, + const Transform3s& tf, + KDOP<18>& bv); + +template <> +COAL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, + const Transform3s& tf, + KDOP<24>& bv); + +template <> +COAL_DLLAPI void computeBV(const Plane& s, const Transform3s& tf, + OBB& bv); + +template <> +COAL_DLLAPI void computeBV(const Plane& s, const Transform3s& tf, + RSS& bv); + +template <> +COAL_DLLAPI void computeBV(const Plane& s, const Transform3s& tf, + OBBRSS& bv); + +template <> +COAL_DLLAPI void computeBV(const Plane& s, const Transform3s& tf, + kIOS& bv); + +template <> +COAL_DLLAPI void computeBV, Plane>(const Plane& s, + const Transform3s& tf, + KDOP<16>& bv); + +template <> +COAL_DLLAPI void computeBV, Plane>(const Plane& s, + const Transform3s& tf, + KDOP<18>& bv); + +template <> +COAL_DLLAPI void computeBV, Plane>(const Plane& s, + const Transform3s& tf, + KDOP<24>& bv); + +/// @brief construct a box shape (with a configuration) from a given bounding +/// volume +COAL_DLLAPI void constructBox(const AABB& bv, Box& box, Transform3s& tf); + +COAL_DLLAPI void constructBox(const OBB& bv, Box& box, Transform3s& tf); + +COAL_DLLAPI void constructBox(const OBBRSS& bv, Box& box, Transform3s& tf); + +COAL_DLLAPI void constructBox(const kIOS& bv, Box& box, Transform3s& tf); + +COAL_DLLAPI void constructBox(const RSS& bv, Box& box, Transform3s& tf); + +COAL_DLLAPI void constructBox(const KDOP<16>& bv, Box& box, Transform3s& tf); + +COAL_DLLAPI void constructBox(const KDOP<18>& bv, Box& box, Transform3s& tf); + +COAL_DLLAPI void constructBox(const KDOP<24>& bv, Box& box, Transform3s& tf); + +COAL_DLLAPI void constructBox(const AABB& bv, const Transform3s& tf_bv, + Box& box, Transform3s& tf); + +COAL_DLLAPI void constructBox(const OBB& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf); + +COAL_DLLAPI void constructBox(const OBBRSS& bv, const Transform3s& tf_bv, + Box& box, Transform3s& tf); + +COAL_DLLAPI void constructBox(const kIOS& bv, const Transform3s& tf_bv, + Box& box, Transform3s& tf); + +COAL_DLLAPI void constructBox(const RSS& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf); + +COAL_DLLAPI void constructBox(const KDOP<16>& bv, const Transform3s& tf_bv, + Box& box, Transform3s& tf); + +COAL_DLLAPI void constructBox(const KDOP<18>& bv, const Transform3s& tf_bv, + Box& box, Transform3s& tf); + +COAL_DLLAPI void constructBox(const KDOP<24>& bv, const Transform3s& tf_bv, + Box& box, Transform3s& tf); + +COAL_DLLAPI Halfspace transform(const Halfspace& a, const Transform3s& tf); + +COAL_DLLAPI Plane transform(const Plane& a, const Transform3s& tf); + +COAL_DLLAPI std::array transformToHalfspaces( + const Plane& a, const Transform3s& tf); + +} // namespace coal + +#endif diff --git a/include/coal/timings.h b/include/coal/timings.h new file mode 100644 index 000000000..c9de3e3ee --- /dev/null +++ b/include/coal/timings.h @@ -0,0 +1,112 @@ +// +// Copyright (c) 2021-2023 INRIA +// + +#ifndef COAL_TIMINGS_FWD_H +#define COAL_TIMINGS_FWD_H + +#include "coal/fwd.hh" + +#ifdef COAL_WITH_CXX11_SUPPORT +#include +#endif + +namespace coal { + +struct CPUTimes { + double wall; + double user; + double system; + + CPUTimes() : wall(0), user(0), system(0) {} + + void clear() { wall = user = system = 0; } +}; + +/// +/// @brief This class mimics the way "boost/timer/timer.hpp" operates while +/// using the modern std::chrono library. +/// Importantly, this class will only have an effect for C++11 and more. +/// +struct COAL_DLLAPI Timer { +#ifdef COAL_WITH_CXX11_SUPPORT + typedef std::chrono::steady_clock clock_type; + typedef clock_type::duration duration_type; +#endif + + /// \brief Default constructor for the timer + /// + /// \param[in] start_on_construction if true, the timer will be run just after + /// the object is created + Timer(const bool start_on_construction = true) : m_is_stopped(true) { + if (start_on_construction) Timer::start(); + } + + CPUTimes elapsed() const { + if (m_is_stopped) return m_times; + + CPUTimes current(m_times); +#ifdef COAL_WITH_CXX11_SUPPORT + std::chrono::time_point current_clock = + std::chrono::steady_clock::now(); + current.user += static_cast( + std::chrono::duration_cast( + current_clock - m_start) + .count()) * + 1e-3; +#endif + return current; + } + +#ifdef COAL_WITH_CXX11_SUPPORT + duration_type duration() const { return (m_end - m_start); } +#endif + + void start() { + if (m_is_stopped) { + m_is_stopped = false; + m_times.clear(); + +#ifdef COAL_WITH_CXX11_SUPPORT + m_start = std::chrono::steady_clock::now(); +#endif + } + } + + void stop() { + if (m_is_stopped) return; + m_is_stopped = true; + +#ifdef COAL_WITH_CXX11_SUPPORT + m_end = std::chrono::steady_clock::now(); + m_times.user += static_cast( + std::chrono::duration_cast( + m_end - m_start) + .count()) * + 1e-3; +#endif + } + + void resume() { +#ifdef COAL_WITH_CXX11_SUPPORT + if (m_is_stopped) { + m_start = std::chrono::steady_clock::now(); + m_is_stopped = false; + } +#endif + } + + bool is_stopped() const { return m_is_stopped; } + + protected: + CPUTimes m_times; + bool m_is_stopped; + +#ifdef COAL_WITH_CXX11_SUPPORT + std::chrono::time_point m_start, m_end; +#endif +}; + +} // namespace coal + +#endif // ifndef COAL_TIMINGS_FWD_H diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h index bdcc8de64..c76ec66b0 100644 --- a/include/hpp/fcl/BV/AABB.h +++ b/include/hpp/fcl/BV/AABB.h @@ -1,269 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_AABB_H -#define HPP_FCL_AABB_H - -#include "hpp/fcl/data_types.h" - -namespace hpp { -namespace fcl { - -struct CollisionRequest; -class Plane; -class Halfspace; - -/// @defgroup Bounding_Volume Bounding volumes -/// Classes of different types of bounding volume. -/// @{ - -/// @brief A class describing the AABB collision structure, which is a box in 3D -/// space determined by two diagonal points -class HPP_FCL_DLLAPI AABB { - public: - /// @brief The min point in the AABB - Vec3f min_; - /// @brief The max point in the AABB - Vec3f max_; - - /// @brief Creating an AABB with zero size (low bound +inf, upper bound -inf) - AABB(); - - /// @brief Creating an AABB at position v with zero size - AABB(const Vec3f& v) : min_(v), max_(v) {} - - /// @brief Creating an AABB with two endpoints a and b - AABB(const Vec3f& a, const Vec3f& b) - : min_(a.cwiseMin(b)), max_(a.cwiseMax(b)) {} - - /// @brief Creating an AABB centered as core and is of half-dimension delta - AABB(const AABB& core, const Vec3f& delta) - : min_(core.min_ - delta), max_(core.max_ + delta) {} - - /// @brief Creating an AABB contains three points - AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) - : min_(a.cwiseMin(b).cwiseMin(c)), max_(a.cwiseMax(b).cwiseMax(c)) {} - - AABB(const AABB& other) = default; - - AABB& operator=(const AABB& other) = default; - - AABB& update(const Vec3f& a, const Vec3f& b) { - min_ = a.cwiseMin(b); - max_ = a.cwiseMax(b); - return *this; - } - - /// @brief Comparison operator - bool operator==(const AABB& other) const { - return min_ == other.min_ && max_ == other.max_; - } - - bool operator!=(const AABB& other) const { return !(*this == other); } - - /// @name Bounding volume API - /// Common API to BVs. - /// @{ - - /// @brief Check whether the AABB contains a point - inline bool contain(const Vec3f& p) const { - if (p[0] < min_[0] || p[0] > max_[0]) return false; - if (p[1] < min_[1] || p[1] > max_[1]) return false; - if (p[2] < min_[2] || p[2] > max_[2]) return false; - - return true; - } - - /// @brief Check whether two AABB are overlap - inline bool overlap(const AABB& other) const { - if (min_[0] > other.max_[0]) return false; - if (min_[1] > other.max_[1]) return false; - if (min_[2] > other.max_[2]) return false; - - if (max_[0] < other.min_[0]) return false; - if (max_[1] < other.min_[1]) return false; - if (max_[2] < other.min_[2]) return false; - - return true; - } - - /// @brief Check whether AABB overlaps a plane - bool overlap(const Plane& p) const; - - /// @brief Check whether AABB overlaps a halfspace - bool overlap(const Halfspace& hs) const; - - /// @brief Check whether two AABB are overlap - bool overlap(const AABB& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const; - - /// @brief Distance between two AABBs - FCL_REAL distance(const AABB& other) const; - - /// @brief Distance between two AABBs; P and Q, should not be NULL, return the - /// nearest points - FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const; - - /// @brief Merge the AABB and a point - inline AABB& operator+=(const Vec3f& p) { - min_ = min_.cwiseMin(p); - max_ = max_.cwiseMax(p); - return *this; - } - - /// @brief Merge the AABB and another AABB - inline AABB& operator+=(const AABB& other) { - min_ = min_.cwiseMin(other.min_); - max_ = max_.cwiseMax(other.max_); - return *this; - } - - /// @brief Return the merged AABB of current AABB and the other one - inline AABB operator+(const AABB& other) const { - AABB res(*this); - return res += other; - } - - /// @brief Size of the AABB (used in BV_Splitter to order two AABBs) - inline FCL_REAL size() const { return (max_ - min_).squaredNorm(); } - - /// @brief Center of the AABB - inline Vec3f center() const { return (min_ + max_) * 0.5; } - - /// @brief Width of the AABB - inline FCL_REAL width() const { return max_[0] - min_[0]; } - - /// @brief Height of the AABB - inline FCL_REAL height() const { return max_[1] - min_[1]; } - - /// @brief Depth of the AABB - inline FCL_REAL depth() const { return max_[2] - min_[2]; } - - /// @brief Volume of the AABB - inline FCL_REAL volume() const { return width() * height() * depth(); } - - /// @} - - /// @brief Check whether the AABB contains another AABB - inline bool contain(const AABB& other) const { - return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && - (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && - (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]); - } - - /// @brief Check whether two AABB are overlap and return the overlap part - inline bool overlap(const AABB& other, AABB& overlap_part) const { - if (!overlap(other)) { - return false; - } - - overlap_part.min_ = min_.cwiseMax(other.min_); - overlap_part.max_ = max_.cwiseMin(other.max_); - return true; - } - - /// @brief Check whether two AABB are overlapped along specific axis - inline bool axisOverlap(const AABB& other, int axis_id) const { - if (min_[axis_id] > other.max_[axis_id]) return false; - if (max_[axis_id] < other.min_[axis_id]) return false; - - return true; - } - - /// @brief expand the half size of the AABB by delta, and keep the center - /// unchanged. - inline AABB& expand(const Vec3f& delta) { - min_ -= delta; - max_ += delta; - return *this; - } - - /// @brief expand the half size of the AABB by a scalar delta, and keep the - /// center unchanged. - inline AABB& expand(const FCL_REAL delta) { - min_.array() -= delta; - max_.array() += delta; - return *this; - } - - /// @brief expand the aabb by increase the thickness of the plate by a ratio - inline AABB& expand(const AABB& core, FCL_REAL ratio) { - min_ = min_ * ratio - core.min_; - max_ = max_ * ratio - core.max_; - return *this; - } - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/// @brief translate the center of AABB by t -static inline AABB translate(const AABB& aabb, const Vec3f& t) { - AABB res(aabb); - res.min_ += t; - res.max_ += t; - return res; -} - -static inline AABB rotate(const AABB& aabb, const Matrix3f& R) { - AABB res(R * aabb.min_); - Vec3f corner(aabb.min_); - const Eigen::DenseIndex bit[3] = {1, 2, 4}; - for (Eigen::DenseIndex ic = 1; ic < 8; - ++ic) { // ic = 0 corresponds to aabb.min_. Skip it. - for (Eigen::DenseIndex i = 0; i < 3; ++i) { - corner[i] = (ic & bit[i]) ? aabb.max_[i] : aabb.min_[i]; - } - res += R * corner; - } - return res; -} - -/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) -/// and b2 is in identity. -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, - const AABB& b2); - -/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) -/// and b2 is in identity. -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, - const AABB& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound); -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/BV/BV.h b/include/hpp/fcl/BV/BV.h index b42316ea0..cda871c3a 100644 --- a/include/hpp/fcl/BV/BV.h +++ b/include/hpp/fcl/BV/BV.h @@ -1,292 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_BV_H -#define HPP_FCL_BV_H - -#include -#include -#include -#include -#include -#include -#include - -/** @brief Main namespace */ -namespace hpp { -namespace fcl { - -/// @cond IGNORE -namespace details { - -/// @brief Convert a bounding volume of type BV1 in configuration tf1 to a -/// bounding volume of type BV2 in I configuration. -template -struct Converter { - static void convert(const BV1& bv1, const Transform3f& tf1, BV2& bv2); - static void convert(const BV1& bv1, BV2& bv2); -}; - -/// @brief Convert from AABB to AABB, not very tight but is fast. -template <> -struct Converter { - static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2) { - const Vec3f& center = bv1.center(); - FCL_REAL r = (bv1.max_ - bv1.min_).norm() * 0.5; - const Vec3f center2 = tf1.transform(center); - bv2.min_ = center2 - Vec3f::Constant(r); - bv2.max_ = center2 + Vec3f::Constant(r); - } - - static void convert(const AABB& bv1, AABB& bv2) { bv2 = bv1; } -}; - -template <> -struct Converter { - static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2) { - bv2.To = tf1.transform(bv1.center()); - bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5; - bv2.axes = tf1.getRotation(); - } - - static void convert(const AABB& bv1, OBB& bv2) { - bv2.To = bv1.center(); - bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5; - bv2.axes.setIdentity(); - } -}; - -template <> -struct Converter { - static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2) { - bv2.extent = bv1.extent; - bv2.To = tf1.transform(bv1.To); - bv2.axes.noalias() = tf1.getRotation() * bv1.axes; - } - - static void convert(const OBB& bv1, OBB& bv2) { bv2 = bv1; } -}; - -template <> -struct Converter { - static void convert(const OBBRSS& bv1, const Transform3f& tf1, OBB& bv2) { - Converter::convert(bv1.obb, tf1, bv2); - } - - static void convert(const OBBRSS& bv1, OBB& bv2) { - Converter::convert(bv1.obb, bv2); - } -}; - -template <> -struct Converter { - static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2) { - bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius, - bv1.length[1] * 0.5 + bv1.radius, bv1.radius); - bv2.To = tf1.transform(bv1.Tr); - bv2.axes.noalias() = tf1.getRotation() * bv1.axes; - } - - static void convert(const RSS& bv1, OBB& bv2) { - bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius, - bv1.length[1] * 0.5 + bv1.radius, bv1.radius); - bv2.To = bv1.Tr; - bv2.axes = bv1.axes; - } -}; - -template -struct Converter { - static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2) { - const Vec3f& center = bv1.center(); - FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; - const Vec3f center2 = tf1.transform(center); - bv2.min_ = center2 - Vec3f::Constant(r); - bv2.max_ = center2 + Vec3f::Constant(r); - } - - static void convert(const BV1& bv1, AABB& bv2) { - const Vec3f& center = bv1.center(); - FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; - bv2.min_ = center - Vec3f::Constant(r); - bv2.max_ = center + Vec3f::Constant(r); - } -}; - -template -struct Converter { - static void convert(const BV1& bv1, const Transform3f& tf1, OBB& bv2) { - AABB bv; - Converter::convert(bv1, bv); - Converter::convert(bv, tf1, bv2); - } - - static void convert(const BV1& bv1, OBB& bv2) { - AABB bv; - Converter::convert(bv1, bv); - Converter::convert(bv, bv2); - } -}; - -template <> -struct Converter { - static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2) { - bv2.Tr = tf1.transform(bv1.To); - bv2.axes.noalias() = tf1.getRotation() * bv1.axes; - - bv2.radius = bv1.extent[2]; - bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius); - bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius); - } - - static void convert(const OBB& bv1, RSS& bv2) { - bv2.Tr = bv1.To; - bv2.axes = bv1.axes; - - bv2.radius = bv1.extent[2]; - bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius); - bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius); - } -}; - -template <> -struct Converter { - static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2) { - bv2.Tr = tf1.transform(bv1.Tr); - bv2.axes.noalias() = tf1.getRotation() * bv1.axes; - - bv2.radius = bv1.radius; - bv2.length[0] = bv1.length[0]; - bv2.length[1] = bv1.length[1]; - } - - static void convert(const RSS& bv1, RSS& bv2) { bv2 = bv1; } -}; - -template <> -struct Converter { - static void convert(const OBBRSS& bv1, const Transform3f& tf1, RSS& bv2) { - Converter::convert(bv1.rss, tf1, bv2); - } - - static void convert(const OBBRSS& bv1, RSS& bv2) { - Converter::convert(bv1.rss, bv2); - } -}; - -template <> -struct Converter { - static void convert(const AABB& bv1, const Transform3f& tf1, RSS& bv2) { - bv2.Tr = tf1.transform(bv1.center()); - - /// Sort the AABB edges so that AABB extents are ordered. - FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth()}; - Eigen::DenseIndex id[3] = {0, 1, 2}; - - for (Eigen::DenseIndex i = 1; i < 3; ++i) { - for (Eigen::DenseIndex j = i; j > 0; --j) { - if (d[j] > d[j - 1]) { - { - FCL_REAL tmp = d[j]; - d[j] = d[j - 1]; - d[j - 1] = tmp; - } - { - Eigen::DenseIndex tmp = id[j]; - id[j] = id[j - 1]; - id[j - 1] = tmp; - } - } - } - } - - const Vec3f extent = (bv1.max_ - bv1.min_) * 0.5; - bv2.radius = extent[id[2]]; - bv2.length[0] = (extent[id[0]] - bv2.radius) * 2; - bv2.length[1] = (extent[id[1]] - bv2.radius) * 2; - - const Matrix3f& R = tf1.getRotation(); - const bool left_hand = (id[0] == (id[1] + 1) % 3); - if (left_hand) - bv2.axes.col(0) = -R.col(id[0]); - else - bv2.axes.col(0) = R.col(id[0]); - bv2.axes.col(1) = R.col(id[1]); - bv2.axes.col(2) = R.col(id[2]); - } - - static void convert(const AABB& bv1, RSS& bv2) { - convert(bv1, Transform3f(), bv2); - } -}; - -template <> -struct Converter { - static void convert(const AABB& bv1, const Transform3f& tf1, OBBRSS& bv2) { - Converter::convert(bv1, tf1, bv2.obb); - Converter::convert(bv1, tf1, bv2.rss); - } - - static void convert(const AABB& bv1, OBBRSS& bv2) { - Converter::convert(bv1, bv2.obb); - Converter::convert(bv1, bv2.rss); - } -}; - -} // namespace details - -/// @endcond - -/// @brief Convert a bounding volume of type BV1 in configuration tf1 to -/// bounding volume of type BV2 in identity configuration. -template -static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2) { - details::Converter::convert(bv1, tf1, bv2); -} - -/// @brief Convert a bounding volume of type BV1 to bounding volume of type BV2 -/// in identity configuration. -template -static inline void convertBV(const BV1& bv1, BV2& bv2) { - details::Converter::convert(bv1, bv2); -} - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/BV/BV_node.h b/include/hpp/fcl/BV/BV_node.h index 300b24ecc..50e3d7adc 100644 --- a/include/hpp/fcl/BV/BV_node.h +++ b/include/hpp/fcl/BV/BV_node.h @@ -1,169 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_BV_NODE_H -#define HPP_FCL_BV_NODE_H - -#include -#include - -namespace hpp { -namespace fcl { - -/// @defgroup Construction_Of_BVH Construction of BVHModel -/// Classes which are used to build a BVHModel (Bounding Volume Hierarchy) -/// @{ - -/// @brief BVNodeBase encodes the tree structure for BVH -struct HPP_FCL_DLLAPI BVNodeBase { - /// @brief An index for first child node or primitive - /// If the value is positive, it is the index of the first child bv node - /// If the value is negative, it is -(primitive index + 1) - /// Zero is not used. - int first_child; - - /// @brief The start id the primitive belonging to the current node. The index - /// is referred to the primitive_indices in BVHModel and from that we can - /// obtain the primitive's index in original data indirectly. - unsigned int first_primitive; - - /// @brief The number of primitives belonging to the current node - unsigned int num_primitives; - - /// @brief Default constructor - BVNodeBase() - : first_child(0), - first_primitive( - (std::numeric_limits::max)()) // value we should help - // to raise an issue - , - num_primitives(0) {} - - /// @brief Equality operator - bool operator==(const BVNodeBase& other) const { - return first_child == other.first_child && - first_primitive == other.first_primitive && - num_primitives == other.num_primitives; - } - - /// @brief Difference operator - bool operator!=(const BVNodeBase& other) const { return !(*this == other); } - - /// @brief Whether current node is a leaf node (i.e. contains a primitive - /// index - inline bool isLeaf() const { return first_child < 0; } - - /// @brief Return the primitive index. The index is referred to the original - /// data (i.e. vertices or tri_indices) in BVHModel - inline int primitiveId() const { return -(first_child + 1); } - - /// @brief Return the index of the first child. The index is referred to the - /// bounding volume array (i.e. bvs) in BVHModel - inline int leftChild() const { return first_child; } - - /// @brief Return the index of the second child. The index is referred to the - /// bounding volume array (i.e. bvs) in BVHModel - inline int rightChild() const { return first_child + 1; } -}; - -/// @brief A class describing a bounding volume node. It includes the tree -/// structure providing in BVNodeBase and also the geometry data provided in BV -/// template parameter. -template -struct HPP_FCL_DLLAPI BVNode : public BVNodeBase { - typedef BVNodeBase Base; - - /// @brief bounding volume storing the geometry - BV bv; - - /// @brief Equality operator - bool operator==(const BVNode& other) const { - return Base::operator==(other) && bv == other.bv; - } - - /// @brief Difference operator - bool operator!=(const BVNode& other) const { return !(*this == other); } - - /// @brief Check whether two BVNode collide - bool overlap(const BVNode& other) const { return bv.overlap(other.bv); } - /// @brief Check whether two BVNode collide - bool overlap(const BVNode& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const { - return bv.overlap(other.bv, request, sqrDistLowerBound); - } - - /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and - /// the underlying BV supports distance, return the nearest points. - FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL, - Vec3f* P2 = NULL) const { - return bv.distance(other.bv, P1, P2); - } - - /// @brief Access to the center of the BV - Vec3f getCenter() const { return bv.center(); } - - /// @brief Access to the orientation of the BV - const Matrix3f& getOrientation() const { - static const Matrix3f id3 = Matrix3f::Identity(); - return id3; - } - - /// \cond - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - /// \endcond -}; - -template <> -inline const Matrix3f& BVNode::getOrientation() const { - return bv.axes; -} - -template <> -inline const Matrix3f& BVNode::getOrientation() const { - return bv.axes; -} - -template <> -inline const Matrix3f& BVNode::getOrientation() const { - return bv.obb.axes; -} - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/BV/OBB.h b/include/hpp/fcl/BV/OBB.h index a8e11a006..003da1750 100644 --- a/include/hpp/fcl/BV/OBB.h +++ b/include/hpp/fcl/BV/OBB.h @@ -1,153 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_OBB_H -#define HPP_FCL_OBB_H - -#include "hpp/fcl/data_types.h" - -namespace hpp { -namespace fcl { - -struct CollisionRequest; - -/// @addtogroup Bounding_Volume -/// @{ - -/// @brief Oriented bounding box class -struct HPP_FCL_DLLAPI OBB { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /// @brief Orientation of OBB. axis[i] is the ith column of the orientation - /// matrix for the box; it is also the i-th principle direction of the box. We - /// assume that axis[0] corresponds to the axis with the longest box edge, - /// axis[1] corresponds to the shorter one and axis[2] corresponds to the - /// shortest one. - Matrix3f axes; - - /// @brief Center of OBB - Vec3f To; - - /// @brief Half dimensions of OBB - Vec3f extent; - - OBB() : axes(Matrix3f::Zero()), To(Vec3f::Zero()), extent(Vec3f::Zero()) {} - - /// @brief Equality operator - bool operator==(const OBB& other) const { - return axes == other.axes && To == other.To && extent == other.extent; - } - - /// @brief Difference operator - bool operator!=(const OBB& other) const { return !(*this == other); } - - /// @brief Check whether the OBB contains a point. - bool contain(const Vec3f& p) const; - - /// Check collision between two OBB - /// @return true if collision happens. - bool overlap(const OBB& other) const; - - /// Check collision between two OBB - /// @return true if collision happens. - /// @retval sqrDistLowerBound squared lower bound on distance between boxes if - /// they do not overlap. - bool overlap(const OBB& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const; - - /// @brief Distance between two OBBs, not implemented. - FCL_REAL distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; - - /// @brief A simple way to merge the OBB and a point (the result is not - /// compact). - OBB& operator+=(const Vec3f& p); - - /// @brief Merge the OBB and another OBB (the result is not compact). - OBB& operator+=(const OBB& other) { - *this = *this + other; - return *this; - } - - /// @brief Return the merged OBB of current OBB and the other one (the result - /// is not compact). - OBB operator+(const OBB& other) const; - - /// @brief Size of the OBB (used in BV_Splitter to order two OBBs) - inline FCL_REAL size() const { return extent.squaredNorm(); } - - /// @brief Center of the OBB - inline const Vec3f& center() const { return To; } - - /// @brief Width of the OBB. - inline FCL_REAL width() const { return 2 * extent[0]; } - - /// @brief Height of the OBB. - inline FCL_REAL height() const { return 2 * extent[1]; } - - /// @brief Depth of the OBB - inline FCL_REAL depth() const { return 2 * extent[2]; } - - /// @brief Volume of the OBB - inline FCL_REAL volume() const { return width() * height() * depth(); } -}; - -/// @brief Translate the OBB bv -HPP_FCL_DLLAPI OBB translate(const OBB& bv, const Vec3f& t); - -/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and -/// b2 is in identity. -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, - const OBB& b2); - -/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and -/// b2 is in identity. -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, - const OBB& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound); - -/// Check collision between two boxes -/// @param B, T orientation and position of first box, -/// @param a half dimensions of first box, -/// @param b half dimensions of second box. -/// The second box is in identity configuration. -HPP_FCL_DLLAPI bool obbDisjoint(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b); -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/BV/OBBRSS.h b/include/hpp/fcl/BV/OBBRSS.h index d224ee162..16b21dbda 100644 --- a/include/hpp/fcl/BV/OBBRSS.h +++ b/include/hpp/fcl/BV/OBBRSS.h @@ -1,162 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_OBBRSS_H -#define HPP_FCL_OBBRSS_H - -#include "hpp/fcl/BV/OBB.h" -#include "hpp/fcl/BV/RSS.h" - -namespace hpp { -namespace fcl { - -struct CollisionRequest; - -/// @addtogroup Bounding_Volume -/// @{ - -/// @brief Class merging the OBB and RSS, can handle collision and distance -/// simultaneously -struct HPP_FCL_DLLAPI OBBRSS { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /// @brief OBB member, for rotation - OBB obb; - - /// @brief RSS member, for distance - RSS rss; - - /// @brief Equality operator - bool operator==(const OBBRSS& other) const { - return obb == other.obb && rss == other.rss; - } - - /// @brief Difference operator - bool operator!=(const OBBRSS& other) const { return !(*this == other); } - - /// @brief Check whether the OBBRSS contains a point - inline bool contain(const Vec3f& p) const { return obb.contain(p); } - - /// @brief Check collision between two OBBRSS - bool overlap(const OBBRSS& other) const { return obb.overlap(other.obb); } - - /// Check collision between two OBBRSS - /// @retval sqrDistLowerBound squared lower bound on distance between - /// objects if they do not overlap. - bool overlap(const OBBRSS& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const { - return obb.overlap(other.obb, request, sqrDistLowerBound); - } - - /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the - /// nearest points - FCL_REAL distance(const OBBRSS& other, Vec3f* P = NULL, - Vec3f* Q = NULL) const { - return rss.distance(other.rss, P, Q); - } - - /// @brief Merge the OBBRSS and a point - OBBRSS& operator+=(const Vec3f& p) { - obb += p; - rss += p; - return *this; - } - - /// @brief Merge two OBBRSS - OBBRSS& operator+=(const OBBRSS& other) { - *this = *this + other; - return *this; - } - - /// @brief Merge two OBBRSS - OBBRSS operator+(const OBBRSS& other) const { - OBBRSS result; - result.obb = obb + other.obb; - result.rss = rss + other.rss; - return result; - } - - /// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS) - inline FCL_REAL size() const { return obb.size(); } - - /// @brief Center of the OBBRSS - inline const Vec3f& center() const { return obb.center(); } - - /// @brief Width of the OBRSS - inline FCL_REAL width() const { return obb.width(); } - - /// @brief Height of the OBBRSS - inline FCL_REAL height() const { return obb.height(); } - - /// @brief Depth of the OBBRSS - inline FCL_REAL depth() const { return obb.depth(); } - - /// @brief Volume of the OBBRSS - inline FCL_REAL volume() const { return obb.volume(); } -}; - -/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) -/// and b2 is in indentity -inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, - const OBBRSS& b2) { - return overlap(R0, T0, b1.obb, b2.obb); -} - -/// Check collision between two OBBRSS -/// @param R0, T0 configuration of b1 -/// @param b1 first OBBRSS in configuration (R0, T0) -/// @param b2 second OBBRSS in identity position -/// @retval sqrDistLowerBound squared lower bound on the distance if OBBRSS do -/// not overlap. -inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, - const OBBRSS& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) { - return overlap(R0, T0, b1.obb, b2.obb, request, sqrDistLowerBound); -} - -/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) -/// and b2 is in indentity; P and Q, is not NULL, returns the nearest points -inline FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, - const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL) { - return distance(R0, T0, b1.rss, b2.rss, P, Q); -} - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/BV/RSS.h b/include/hpp/fcl/BV/RSS.h index 734e20a74..cc6089bab 100644 --- a/include/hpp/fcl/BV/RSS.h +++ b/include/hpp/fcl/BV/RSS.h @@ -1,176 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_RSS_H -#define HPP_FCL_RSS_H - -#include "hpp/fcl/data_types.h" - -#include - -namespace hpp { -namespace fcl { - -struct CollisionRequest; - -/// @addtogroup Bounding_Volume -/// @{ - -/// @brief A class for rectangle sphere-swept bounding volume -struct HPP_FCL_DLLAPI RSS { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /// @brief Orientation of RSS. axis[i] is the ith column of the orientation - /// matrix for the RSS; it is also the i-th principle direction of the RSS. We - /// assume that axis[0] corresponds to the axis with the longest length, - /// axis[1] corresponds to the shorter one and axis[2] corresponds to the - /// shortest one. - Matrix3f axes; - - /// @brief Origin of the rectangle in RSS - Vec3f Tr; - - /// @brief Side lengths of rectangle - FCL_REAL length[2]; - - /// @brief Radius of sphere summed with rectangle to form RSS - FCL_REAL radius; - - ///  @brief Default constructor with default values - RSS() : axes(Matrix3f::Zero()), Tr(Vec3f::Zero()), radius(-1) { - length[0] = 0; - length[1] = 0; - } - - /// @brief Equality operator - bool operator==(const RSS& other) const { - return axes == other.axes && Tr == other.Tr && - length[0] == other.length[0] && length[1] == other.length[1] && - radius == other.radius; - } - - /// @brief Difference operator - bool operator!=(const RSS& other) const { return !(*this == other); } - - /// @brief Check whether the RSS contains a point - bool contain(const Vec3f& p) const; - - /// @brief Check collision between two RSS - bool overlap(const RSS& other) const; - - /// Not implemented - bool overlap(const RSS& other, const CollisionRequest&, - FCL_REAL& sqrDistLowerBound) const { - sqrDistLowerBound = sqrt(-1); - return overlap(other); - } - - /// @brief the distance between two RSS; P and Q, if not NULL, return the - /// nearest points - FCL_REAL distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; - - /// @brief A simple way to merge the RSS and a point, not compact. - /// @todo This function may have some bug. - RSS& operator+=(const Vec3f& p); - - /// @brief Merge the RSS and another RSS - inline RSS& operator+=(const RSS& other) { - *this = *this + other; - return *this; - } - - /// @brief Return the merged RSS of current RSS and the other one - RSS operator+(const RSS& other) const; - - /// @brief Size of the RSS (used in BV_Splitter to order two RSSs) - inline FCL_REAL size() const { - return (std::sqrt(length[0] * length[0] + length[1] * length[1]) + - 2 * radius); - } - - /// @brief The RSS center - inline const Vec3f& center() const { return Tr; } - - /// @brief Width of the RSS - inline FCL_REAL width() const { return length[0] + 2 * radius; } - - /// @brief Height of the RSS - inline FCL_REAL height() const { return length[1] + 2 * radius; } - - /// @brief Depth of the RSS - inline FCL_REAL depth() const { return 2 * radius; } - - /// @brief Volume of the RSS - inline FCL_REAL volume() const { - return (length[0] * length[1] * 2 * radius + - 4 * boost::math::constants::pi() * radius * radius * - radius); - } - - /// @brief Check collision between two RSS and return the overlap part. - /// For RSS, we return nothing, as the overlap part of two RSSs usually is not - /// a RSS. - bool overlap(const RSS& other, RSS& /*overlap_part*/) const { - return overlap(other); - } -}; - -/// @brief distance between two RSS bounding volumes -/// P and Q (optional return values) are the closest points in the rectangles, -/// not the RSS. But the direction P - Q is the correct direction for cloest -/// points Notice that P and Q are both in the local frame of the first RSS (not -/// global frame and not even the local frame of object 1) -HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, - const RSS& b1, const RSS& b2, Vec3f* P = NULL, - Vec3f* Q = NULL); - -/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and -/// b2 is in identity. -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, - const RSS& b2); - -/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and -/// b2 is in identity. -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, - const RSS& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound); - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h index 879bff1f1..51a7c44d2 100644 --- a/include/hpp/fcl/BV/kDOP.h +++ b/include/hpp/fcl/BV/kDOP.h @@ -1,193 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_KDOP_H -#define HPP_FCL_KDOP_H - -#include "hpp/fcl/fwd.hh" -#include "hpp/fcl/data_types.h" - -namespace hpp { -namespace fcl { - -struct CollisionRequest; - -/// @addtogroup Bounding_Volume -/// @{ - -/// @brief KDOP class describes the KDOP collision structures. K is set as the -/// template parameter, which should be 16, 18, or 24 -/// The KDOP structure is defined by some pairs of parallel planes defined by -/// some axes. -/// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off -/// some space of the edges: -/// (-1,0,0) and (1,0,0) -> indices 0 and 8 -/// (0,-1,0) and (0,1,0) -> indices 1 and 9 -/// (0,0,-1) and (0,0,1) -> indices 2 and 10 -/// (-1,-1,0) and (1,1,0) -> indices 3 and 11 -/// (-1,0,-1) and (1,0,1) -> indices 4 and 12 -/// (0,-1,-1) and (0,1,1) -> indices 5 and 13 -/// (-1,1,0) and (1,-1,0) -> indices 6 and 14 -/// (-1,0,1) and (1,0,-1) -> indices 7 and 15 -/// For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off -/// some space of the edges: -/// (-1,0,0) and (1,0,0) -> indices 0 and 9 -/// (0,-1,0) and (0,1,0) -> indices 1 and 10 -/// (0,0,-1) and (0,0,1) -> indices 2 and 11 -/// (-1,-1,0) and (1,1,0) -> indices 3 and 12 -/// (-1,0,-1) and (1,0,1) -> indices 4 and 13 -/// (0,-1,-1) and (0,1,1) -> indices 5 and 14 -/// (-1,1,0) and (1,-1,0) -> indices 6 and 15 -/// (-1,0,1) and (1,0,-1) -> indices 7 and 16 -/// (0,-1,1) and (0,1,-1) -> indices 8 and 17 -/// For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off -/// some space of the edges: -/// (-1,0,0) and (1,0,0) -> indices 0 and 12 -/// (0,-1,0) and (0,1,0) -> indices 1 and 13 -/// (0,0,-1) and (0,0,1) -> indices 2 and 14 -/// (-1,-1,0) and (1,1,0) -> indices 3 and 15 -/// (-1,0,-1) and (1,0,1) -> indices 4 and 16 -/// (0,-1,-1) and (0,1,1) -> indices 5 and 17 -/// (-1,1,0) and (1,-1,0) -> indices 6 and 18 -/// (-1,0,1) and (1,0,-1) -> indices 7 and 19 -/// (0,-1,1) and (0,1,-1) -> indices 8 and 20 -/// (-1, -1, 1) and (1, 1, -1) --> indices 9 and 21 -/// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22 -/// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23 -template -class HPP_FCL_DLLAPI KDOP { - protected: - /// @brief Origin's distances to N KDOP planes - Eigen::Array dist_; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /// @brief Creating kDOP containing nothing - KDOP(); - - /// @brief Creating kDOP containing only one point - KDOP(const Vec3f& v); - - /// @brief Creating kDOP containing two points - KDOP(const Vec3f& a, const Vec3f& b); - - /// @brief Equality operator - bool operator==(const KDOP& other) const { - return (dist_ == other.dist_).all(); - } - - /// @brief Difference operator - bool operator!=(const KDOP& other) const { - return (dist_ != other.dist_).any(); - } - - /// @brief Check whether two KDOPs overlap. - bool overlap(const KDOP& other) const; - - /// @brief Check whether two KDOPs overlap. - /// @return true if collision happens. - /// @retval sqrDistLowerBound squared lower bound on distance between boxes if - /// they do not overlap. - bool overlap(const KDOP& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const; - - /// @brief The distance between two KDOP. Not implemented. - FCL_REAL distance(const KDOP& other, Vec3f* P = NULL, - Vec3f* Q = NULL) const; - - /// @brief Merge the point and the KDOP - KDOP& operator+=(const Vec3f& p); - - /// @brief Merge two KDOPs - KDOP& operator+=(const KDOP& other); - - /// @brief Create a KDOP by mergin two KDOPs - KDOP operator+(const KDOP& other) const; - - /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs) - inline FCL_REAL size() const { - return width() * width() + height() * height() + depth() * depth(); - } - - /// @brief The (AABB) center - inline Vec3f center() const { - return (dist_.template head<3>() + dist_.template segment<3>(N / 2)) / 2; - } - - /// @brief The (AABB) width - inline FCL_REAL width() const { return dist_[N / 2] - dist_[0]; } - - /// @brief The (AABB) height - inline FCL_REAL height() const { return dist_[N / 2 + 1] - dist_[1]; } - - /// @brief The (AABB) depth - inline FCL_REAL depth() const { return dist_[N / 2 + 2] - dist_[2]; } - - /// @brief The (AABB) volume - inline FCL_REAL volume() const { return width() * height() * depth(); } - - inline FCL_REAL dist(short i) const { return dist_[i]; } - - inline FCL_REAL& dist(short i) { return dist_[i]; } - - //// @brief Check whether one point is inside the KDOP - bool inside(const Vec3f& p) const; -}; - -template -bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, - const KDOP& /*b2*/) { - HPP_FCL_THROW_PRETTY("not implemented", std::logic_error); -} - -template -bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, - const KDOP& /*b2*/, const CollisionRequest& /*request*/, - FCL_REAL& /*sqrDistLowerBound*/) { - HPP_FCL_THROW_PRETTY("not implemented", std::logic_error); -} - -/// @brief translate the KDOP BV -template -HPP_FCL_DLLAPI KDOP translate(const KDOP& bv, const Vec3f& t); - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/BV/kIOS.h b/include/hpp/fcl/BV/kIOS.h index 7234b7740..cc24725dd 100644 --- a/include/hpp/fcl/BV/kIOS.h +++ b/include/hpp/fcl/BV/kIOS.h @@ -1,195 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_KIOS_H -#define HPP_FCL_KIOS_H - -#include "hpp/fcl/BV/OBB.h" - -namespace hpp { -namespace fcl { - -struct CollisionRequest; - -/// @addtogroup Bounding_Volume -/// @{ - -/// @brief A class describing the kIOS collision structure, which is a set of -/// spheres. -class HPP_FCL_DLLAPI kIOS { - /// @brief One sphere in kIOS - struct HPP_FCL_DLLAPI kIOS_Sphere { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - Vec3f o; - FCL_REAL r; - - bool operator==(const kIOS_Sphere& other) const { - return o == other.o && r == other.r; - } - - bool operator!=(const kIOS_Sphere& other) const { - return !(*this == other); - } - }; - - /// @brief generate one sphere enclosing two spheres - static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, - const kIOS_Sphere& s1) { - Vec3f d = s1.o - s0.o; - FCL_REAL dist2 = d.squaredNorm(); - FCL_REAL diff_r = s1.r - s0.r; - - /** The sphere with the larger radius encloses the other */ - if (diff_r * diff_r >= dist2) { - if (s1.r > s0.r) - return s1; - else - return s0; - } else /** spheres partially overlapping or disjoint */ - { - float dist = (float)std::sqrt(dist2); - kIOS_Sphere s; - s.r = dist + s0.r + s1.r; - if (dist > 0) - s.o = s0.o + d * ((s.r - s0.r) / dist); - else - s.o = s0.o; - return s; - } - } - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /// @brief Equality operator - bool operator==(const kIOS& other) const { - bool res = obb == other.obb && num_spheres == other.num_spheres; - if (!res) return false; - - for (size_t k = 0; k < num_spheres; ++k) { - if (spheres[k] != other.spheres[k]) return false; - } - - return true; - } - - /// @brief Difference operator - bool operator!=(const kIOS& other) const { return !(*this == other); } - - static constexpr size_t max_num_spheres = 5; - - /// @brief The (at most) five spheres for intersection - kIOS_Sphere spheres[max_num_spheres]; - - /// @brief The number of spheres, no larger than 5 - unsigned int num_spheres; - - /// @ OBB related with kIOS - OBB obb; - - /// @brief Check whether the kIOS contains a point - bool contain(const Vec3f& p) const; - - /// @brief Check collision between two kIOS - bool overlap(const kIOS& other) const; - - /// @brief Check collision between two kIOS - bool overlap(const kIOS& other, const CollisionRequest&, - FCL_REAL& sqrDistLowerBound) const; - - /// @brief The distance between two kIOS - FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; - - /// @brief A simple way to merge the kIOS and a point - kIOS& operator+=(const Vec3f& p); - - /// @brief Merge the kIOS and another kIOS - kIOS& operator+=(const kIOS& other) { - *this = *this + other; - return *this; - } - - /// @brief Return the merged kIOS of current kIOS and the other one - kIOS operator+(const kIOS& other) const; - - /// @brief size of the kIOS (used in BV_Splitter to order two kIOSs) - FCL_REAL size() const; - - /// @brief Center of the kIOS - const Vec3f& center() const { return spheres[0].o; } - - /// @brief Width of the kIOS - FCL_REAL width() const; - - /// @brief Height of the kIOS - FCL_REAL height() const; - - /// @brief Depth of the kIOS - FCL_REAL depth() const; - - /// @brief Volume of the kIOS - FCL_REAL volume() const; -}; - -/// @brief Translate the kIOS BV -HPP_FCL_DLLAPI kIOS translate(const kIOS& bv, const Vec3f& t); - -/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) -/// and b2 is in identity. -/// @todo Not efficient -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, - const kIOS& b2); - -/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) -/// and b2 is in identity. -/// @todo Not efficient -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, - const kIOS& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound); - -/// @brief Approximate distance between two kIOS bounding volumes -/// @todo P and Q is not returned, need implementation -HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, - const kIOS& b1, const kIOS& b2, - Vec3f* P = NULL, Vec3f* Q = NULL); - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/BVH/BVH_front.h b/include/hpp/fcl/BVH/BVH_front.h index 9a3a99eea..0d567079d 100644 --- a/include/hpp/fcl/BVH/BVH_front.h +++ b/include/hpp/fcl/BVH/BVH_front.h @@ -1,79 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_BVH_FRONT_H -#define HPP_FCL_BVH_FRONT_H - -#include - -#include - -namespace hpp { -namespace fcl { - -/// @brief Front list acceleration for collision -/// Front list is a set of internal and leaf nodes in the BVTT hierarchy, where -/// the traversal terminates while performing a query during a given time -/// instance. The front list reflects the subset of a BVTT that is traversed for -/// that particular proximity query. -struct HPP_FCL_DLLAPI BVHFrontNode { - /// @brief The nodes to start in the future, i.e. the wave front of the - /// traversal tree. - unsigned int left, right; - - /// @brief The front node is not valid when collision is detected on the front - /// node. - bool valid; - - BVHFrontNode(unsigned int left_, unsigned int right_) - : left(left_), right(right_), valid(true) {} -}; - -/// @brief BVH front list is a list of front nodes. -typedef std::list BVHFrontList; - -/// @brief Add new front node into the front list -inline void updateFrontList(BVHFrontList* front_list, unsigned int b1, - unsigned int b2) { - if (front_list) front_list->push_back(BVHFrontNode(b1, b2)); -} - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/BVH/BVH_internal.h b/include/hpp/fcl/BVH/BVH_internal.h index 407a430ca..7b24ec33a 100644 --- a/include/hpp/fcl/BVH/BVH_internal.h +++ b/include/hpp/fcl/BVH/BVH_internal.h @@ -1,90 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_BVH_INTERNAL_H -#define HPP_FCL_BVH_INTERNAL_H - -#include - -namespace hpp { -namespace fcl { - -/// @brief States for BVH construction -/// empty->begun->processed ->replace_begun->processed -> ...... -/// | -/// |-> update_begun -> updated -> ..... -enum BVHBuildState { - BVH_BUILD_STATE_EMPTY, /// empty state, immediately after constructor - BVH_BUILD_STATE_BEGUN, /// after beginModel(), state for adding geometry - /// primitives - BVH_BUILD_STATE_PROCESSED, /// after tree has been build, ready for cd use - BVH_BUILD_STATE_UPDATE_BEGUN, /// after beginUpdateModel(), state for - /// updating geometry primitives - BVH_BUILD_STATE_UPDATED, /// after tree has been build for updated geometry, - /// ready for ccd use - BVH_BUILD_STATE_REPLACE_BEGUN /// after beginReplaceModel(), state for - /// replacing geometry primitives -}; - -/// @brief Error code for BVH -enum BVHReturnCode { - BVH_OK = 0, /// BVH is valid - BVH_ERR_MODEL_OUT_OF_MEMORY = - -1, /// Cannot allocate memory for vertices and triangles - BVH_ERR_BUILD_OUT_OF_SEQUENCE = - -2, /// BVH construction does not follow correct sequence - BVH_ERR_BUILD_EMPTY_MODEL = -3, /// BVH geometry is not prepared - BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME = - -4, /// BVH geometry in previous frame is not prepared - BVH_ERR_UNSUPPORTED_FUNCTION = -5, /// BVH funtion is not supported - BVH_ERR_UNUPDATED_MODEL = -6, /// BVH model update failed - BVH_ERR_INCORRECT_DATA = -7, /// BVH data is not valid - BVH_ERR_UNKNOWN = -8 /// Unknown failure -}; - -/// @brief BVH model type -enum BVHModelType { - BVH_MODEL_UNKNOWN, /// @brief unknown model type - BVH_MODEL_TRIANGLES, /// @brief triangle model - BVH_MODEL_POINTCLOUD /// @brief point cloud model -}; - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/BVH/BVH_model.h b/include/hpp/fcl/BVH/BVH_model.h index 34186d0a6..a069ea961 100644 --- a/include/hpp/fcl/BVH/BVH_model.h +++ b/include/hpp/fcl/BVH/BVH_model.h @@ -1,542 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * Copyright (c) 2020-2022, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_BVH_MODEL_H -#define HPP_FCL_BVH_MODEL_H - -#include "hpp/fcl/fwd.hh" -#include "hpp/fcl/collision_object.h" -#include "hpp/fcl/BVH/BVH_internal.h" -#include "hpp/fcl/BV/BV_node.h" - -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// @addtogroup Construction_Of_BVH -/// @{ - -class ConvexBase; - -template -class BVFitter; -template -class BVSplitter; - -/// @brief A base class describing the bounding hierarchy of a mesh model or a -/// point cloud model (which is viewed as a degraded version of mesh) -class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry { - public: - /// @brief Geometry point data - std::shared_ptr> vertices; - - /// @brief Geometry triangle index data, will be NULL for point clouds - std::shared_ptr> tri_indices; - - /// @brief Geometry point data in previous frame - std::shared_ptr> prev_vertices; - - /// @brief Number of triangles - unsigned int num_tris; - - /// @brief Number of points - unsigned int num_vertices; - - /// @brief The state of BVH building process - BVHBuildState build_state; - - /// @brief Convex representation of this object - shared_ptr convex; - - /// @brief Model type described by the instance - BVHModelType getModelType() const { - if (num_tris && num_vertices) - return BVH_MODEL_TRIANGLES; - else if (num_vertices) - return BVH_MODEL_POINTCLOUD; - else - return BVH_MODEL_UNKNOWN; - } - - /// @brief Constructing an empty BVH - BVHModelBase(); - - /// @brief copy from another BVH - BVHModelBase(const BVHModelBase& other); - - /// @brief deconstruction, delete mesh data related. - virtual ~BVHModelBase() {} - - /// @brief Get the object type: it is a BVH - OBJECT_TYPE getObjectType() const { return OT_BVH; } - - /// @brief Compute the AABB for the BVH, used for broad-phase collision - void computeLocalAABB(); - - /// @brief Begin a new BVH model - int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0); - - /// @brief Add one point in the new BVH model - int addVertex(const Vec3f& p); - - /// @brief Add points in the new BVH model - int addVertices(const Matrixx3f& points); - - /// @brief Add triangles in the new BVH model - int addTriangles(const Matrixx3i& triangles); - - /// @brief Add one triangle in the new BVH model - int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3); - - /// @brief Add a set of triangles in the new BVH model - int addSubModel(const std::vector& ps, - const std::vector& ts); - - /// @brief Add a set of points in the new BVH model - int addSubModel(const std::vector& ps); - - /// @brief End BVH model construction, will build the bounding volume - /// hierarchy - int endModel(); - - /// @brief Replace the geometry information of current frame (i.e. should have - /// the same mesh topology with the previous frame) - int beginReplaceModel(); - - /// @brief Replace one point in the old BVH model - int replaceVertex(const Vec3f& p); - - /// @brief Replace one triangle in the old BVH model - int replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3); - - /// @brief Replace a set of points in the old BVH model - int replaceSubModel(const std::vector& ps); - - /// @brief End BVH model replacement, will also refit or rebuild the bounding - /// volume hierarchy - int endReplaceModel(bool refit = true, bool bottomup = true); - - /// @brief Replace the geometry information of current frame (i.e. should have - /// the same mesh topology with the previous frame). The current frame will be - /// saved as the previous frame in prev_vertices. - int beginUpdateModel(); - - /// @brief Update one point in the old BVH model - int updateVertex(const Vec3f& p); - - /// @brief Update one triangle in the old BVH model - int updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3); - - /// @brief Update a set of points in the old BVH model - int updateSubModel(const std::vector& ps); - - /// @brief End BVH model update, will also refit or rebuild the bounding - /// volume hierarchy - int endUpdateModel(bool refit = true, bool bottomup = true); - - /// @brief Build this \ref Convex "Convex" representation of this - /// model. The result is stored in attribute \ref convex. \note this only - /// takes the points of this model. It does not check that the - /// object is convex. It does not compute a convex hull. - void buildConvexRepresentation(bool share_memory); - - /// @brief Build a convex hull - /// and store it in attribute \ref convex. - /// \param keepTriangle whether the convex should be triangulated. - /// \param qhullCommand see \ref ConvexBase::convexHull. - /// \return \c true if this object is convex, hence the convex hull represents - /// the same object. - /// \sa ConvexBase::convexHull - /// \warning At the moment, the return value only checks whether there are as - /// many points in the convex hull as in the original object. This is - /// neither necessary (duplicated vertices get merged) nor sufficient - /// (think of a U with 4 vertices and 3 edges). - bool buildConvexHull(bool keepTriangle, const char* qhullCommand = NULL); - - virtual int memUsage(const bool msg = false) const = 0; - - /// @brief This is a special acceleration: BVH_model default stores the BV's - /// transform in world coordinate. However, we can also store each BV's - /// transform related to its parent BV node. When traversing the BVH, this can - /// save one matrix transformation. - virtual void makeParentRelative() = 0; - - Vec3f computeCOM() const { - FCL_REAL vol = 0; - Vec3f com(0, 0, 0); - if (!(vertices.get())) { - std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " - "vertices." - << std::endl; - return com; - } - const std::vector& vertices_ = *vertices; - if (!(tri_indices.get())) { - std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " - "triangles." - << std::endl; - return com; - } - const std::vector& tri_indices_ = *tri_indices; - - for (unsigned int i = 0; i < num_tris; ++i) { - const Triangle& tri = tri_indices_[i]; - FCL_REAL d_six_vol = - (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]); - vol += d_six_vol; - com += (vertices_[tri[0]] + vertices_[tri[1]] + vertices_[tri[2]]) * - d_six_vol; - } - - return com / (vol * 4); - } - - FCL_REAL computeVolume() const { - FCL_REAL vol = 0; - if (!(vertices.get())) { - std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " - "vertices." - << std::endl; - return vol; - } - const std::vector& vertices_ = *vertices; - if (!(tri_indices.get())) { - std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " - "triangles." - << std::endl; - return vol; - } - const std::vector& tri_indices_ = *tri_indices; - for (unsigned int i = 0; i < num_tris; ++i) { - const Triangle& tri = tri_indices_[i]; - FCL_REAL d_six_vol = - (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]); - vol += d_six_vol; - } - - return vol / 6; - } - - Matrix3f computeMomentofInertia() const { - Matrix3f C = Matrix3f::Zero(); - - Matrix3f C_canonical; - C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0, - 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0; - - if (!(vertices.get())) { - std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does " - "not contain vertices." - << std::endl; - return C; - } - const std::vector& vertices_ = *vertices; - if (!(vertices.get())) { - std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does " - "not contain vertices." - << std::endl; - return C; - } - const std::vector& tri_indices_ = *tri_indices; - for (unsigned int i = 0; i < num_tris; ++i) { - const Triangle& tri = tri_indices_[i]; - const Vec3f& v1 = vertices_[tri[0]]; - const Vec3f& v2 = vertices_[tri[1]]; - const Vec3f& v3 = vertices_[tri[2]]; - Matrix3f A; - A << v1.transpose(), v2.transpose(), v3.transpose(); - C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3); - } - - return C.trace() * Matrix3f::Identity() - C; - } - - protected: - virtual void deleteBVs() = 0; - virtual bool allocateBVs() = 0; - - /// @brief Build the bounding volume hierarchy - virtual int buildTree() = 0; - - /// @brief Refit the bounding volume hierarchy - virtual int refitTree(bool bottomup) = 0; - - unsigned int num_tris_allocated; - unsigned int num_vertices_allocated; - unsigned int num_vertex_updated; /// for ccd vertex update - - protected: - /// \brief Comparison operators - virtual bool isEqual(const CollisionGeometry& other) const; -}; - -/// @brief A class describing the bounding hierarchy of a mesh model or a point -/// cloud model (which is viewed as a degraded version of mesh) \tparam BV one -/// of the bounding volume class in \ref Bounding_Volume. -template -class HPP_FCL_DLLAPI BVHModel : public BVHModelBase { - typedef BVHModelBase Base; - - public: - using bv_node_vector_t = - std::vector, Eigen::aligned_allocator>>; - - /// @brief Split rule to split one BV node into two children - shared_ptr> bv_splitter; - - /// @brief Fitting rule to fit a BV node to a set of geometry primitives - shared_ptr> bv_fitter; - - /// @brief Default constructor to build an empty BVH - BVHModel(); - - /// @brief Copy constructor from another BVH - /// - /// \param[in] other BVHModel to copy. - /// - BVHModel(const BVHModel& other); - - /// @brief Clone *this into a new BVHModel - virtual BVHModel* clone() const { return new BVHModel(*this); } - - /// @brief deconstruction, delete mesh data related. - ~BVHModel() {} - - /// @brief We provide getBV() and getNumBVs() because BVH may be compressed - /// (in future), so we must provide some flexibility here - - /// @brief Access the bv giving the its index - const BVNode& getBV(unsigned int i) const { - assert(i < num_bvs); - return (*bvs)[i]; - } - - /// @brief Access the bv giving the its index - BVNode& getBV(unsigned int i) { - assert(i < num_bvs); - return (*bvs)[i]; - } - - /// @brief Get the number of bv in the BVH - unsigned int getNumBVs() const { return num_bvs; } - - /// @brief Get the BV type: default is unknown - NODE_TYPE getNodeType() const { return BV_UNKNOWN; } - - /// @brief Check the number of memory used - int memUsage(const bool msg) const; - - /// @brief This is a special acceleration: BVH_model default stores the BV's - /// transform in world coordinate. However, we can also store each BV's - /// transform related to its parent BV node. When traversing the BVH, this can - /// save one matrix transformation. - void makeParentRelative() { - Matrix3f I(Matrix3f::Identity()); - makeParentRelativeRecurse(0, I, Vec3f::Zero()); - } - - protected: - void deleteBVs(); - bool allocateBVs(); - - unsigned int num_bvs_allocated; - std::shared_ptr> primitive_indices; - - /// @brief Bounding volume hierarchy - std::shared_ptr bvs; - - /// @brief Number of BV nodes in bounding volume hierarchy - unsigned int num_bvs; - - /// @brief Build the bounding volume hierarchy - int buildTree(); - - /// @brief Refit the bounding volume hierarchy - int refitTree(bool bottomup); - - /// @brief Refit the bounding volume hierarchy in a top-down way (slow but - /// more compact) - int refitTree_topdown(); - - /// @brief Refit the bounding volume hierarchy in a bottom-up way (fast but - /// less compact) - int refitTree_bottomup(); - - /// @brief Recursive kernel for hierarchy construction - int recursiveBuildTree(int bv_id, unsigned int first_primitive, - unsigned int num_primitives); - - /// @brief Recursive kernel for bottomup refitting - int recursiveRefitTree_bottomup(int bv_id); - - /// @ recursively compute each bv's transform related to its parent. For - /// default BV, only the translation works. For oriented BV (OBB, RSS, - /// OBBRSS), special implementation is provided. - void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, - const Vec3f& parent_c) { - bv_node_vector_t& bvs_ = *bvs; - if (!bvs_[static_cast(bv_id)].isLeaf()) { - makeParentRelativeRecurse(bvs_[static_cast(bv_id)].first_child, - parent_axes, - bvs_[static_cast(bv_id)].getCenter()); - - makeParentRelativeRecurse( - bvs_[static_cast(bv_id)].first_child + 1, parent_axes, - bvs_[static_cast(bv_id)].getCenter()); - } - - bvs_[static_cast(bv_id)].bv = - translate(bvs_[static_cast(bv_id)].bv, -parent_c); - } - - private: - virtual bool isEqual(const CollisionGeometry& _other) const { - const BVHModel* other_ptr = dynamic_cast(&_other); - if (other_ptr == nullptr) return false; - const BVHModel& other = *other_ptr; - - bool res = Base::isEqual(other); - if (!res) return false; - - // unsigned int other_num_primitives = 0; - // if(other.primitive_indices) - // { - - // switch(other.getModelType()) - // { - // case BVH_MODEL_TRIANGLES: - // other_num_primitives = num_tris; - // break; - // case BVH_MODEL_POINTCLOUD: - // other_num_primitives = num_vertices; - // break; - // default: - // ; - // } - // } - - // unsigned int num_primitives = 0; - // if(primitive_indices) - // { - // - // switch(other.getModelType()) - // { - // case BVH_MODEL_TRIANGLES: - // num_primitives = num_tris; - // break; - // case BVH_MODEL_POINTCLOUD: - // num_primitives = num_vertices; - // break; - // default: - // ; - // } - // } - // - // if(num_primitives != other_num_primitives) - // return false; - // - // for(int k = 0; k < num_primitives; ++k) - // { - // if(primitive_indices[k] != other.primitive_indices[k]) - // return false; - // } - - if (num_bvs != other.num_bvs) return false; - - if ((!(bvs.get()) && other.bvs.get()) || (bvs.get() && !(other.bvs.get()))) - return false; - if (bvs.get() && other.bvs.get()) { - const bv_node_vector_t& bvs_ = *bvs; - const bv_node_vector_t& other_bvs_ = *(other.bvs); - for (unsigned int k = 0; k < num_bvs; ++k) { - if (bvs_[k] != other_bvs_[k]) return false; - } - } - - return true; - } -}; - -/// @} - -template <> -void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, - const Vec3f& parent_c); - -template <> -void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, - const Vec3f& parent_c); - -template <> -void BVHModel::makeParentRelativeRecurse(int bv_id, - Matrix3f& parent_axes, - const Vec3f& parent_c); - -/// @brief Specialization of getNodeType() for BVHModel with different BV types -template <> -NODE_TYPE BVHModel::getNodeType() const; - -template <> -NODE_TYPE BVHModel::getNodeType() const; - -template <> -NODE_TYPE BVHModel::getNodeType() const; - -template <> -NODE_TYPE BVHModel::getNodeType() const; - -template <> -NODE_TYPE BVHModel::getNodeType() const; - -template <> -NODE_TYPE BVHModel>::getNodeType() const; - -template <> -NODE_TYPE BVHModel>::getNodeType() const; - -template <> -NODE_TYPE BVHModel>::getNodeType() const; - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/BVH/BVH_utility.h b/include/hpp/fcl/BVH/BVH_utility.h index 508af2fec..275a171e1 100644 --- a/include/hpp/fcl/BVH/BVH_utility.h +++ b/include/hpp/fcl/BVH/BVH_utility.h @@ -1,119 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_BVH_UTILITY_H -#define HPP_FCL_BVH_UTILITY_H - -#include - -namespace hpp { -namespace fcl { -/// @brief Extract the part of the BVHModel that is inside an AABB. -/// A triangle in collision with the AABB is considered inside. -template -HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, - const AABB& aabb); - -template <> -HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, - const AABB& aabb); -template <> -HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, - const AABB& aabb); -template <> -HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, - const AABB& aabb); -template <> -HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, - const AABB& aabb); -template <> -HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, - const AABB& aabb); -template <> -HPP_FCL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, - const Transform3f& pose, - const AABB& aabb); -template <> -HPP_FCL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, - const Transform3f& pose, - const AABB& aabb); -template <> -HPP_FCL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, - const Transform3f& pose, - const AABB& aabb); - -/// @brief Compute the covariance matrix for a set or subset of points. if ts = -/// null, then indices refer to points directly; otherwise refer to triangles -HPP_FCL_DLLAPI void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, - unsigned int* indices, unsigned int n, - Matrix3f& M); - -/// @brief Compute the RSS bounding volume parameters: radius, rectangle size -/// and the origin, given the BV axises. -HPP_FCL_DLLAPI void getRadiusAndOriginAndRectangleSize( - Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, - const Matrix3f& axes, Vec3f& origin, FCL_REAL l[2], FCL_REAL& r); - -/// @brief Compute the bounding volume extent and center for a set or subset of -/// points, given the BV axises. -HPP_FCL_DLLAPI void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, - unsigned int* indices, unsigned int n, - Matrix3f& axes, Vec3f& center, - Vec3f& extent); - -/// @brief Compute the center and radius for a triangle's circumcircle -HPP_FCL_DLLAPI void circumCircleComputation(const Vec3f& a, const Vec3f& b, - const Vec3f& c, Vec3f& center, - FCL_REAL& radius); - -/// @brief Compute the maximum distance from a given center point to a point -/// cloud -HPP_FCL_DLLAPI FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, - unsigned int* indices, unsigned int n, - const Vec3f& query); - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase.h b/include/hpp/fcl/broadphase/broadphase.h index 762792de3..7834fbe4c 100644 --- a/include/hpp/fcl/broadphase/broadphase.h +++ b/include/hpp/fcl/broadphase/broadphase.h @@ -1,48 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Inria - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef HPP_FCL_BROADPHASE_BROADPHASE_H -#define HPP_FCL_BROADPHASE_BROADPHASE_H - -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" -#include "hpp/fcl/broadphase/broadphase_bruteforce.h" -#include "hpp/fcl/broadphase/broadphase_SaP.h" -#include "hpp/fcl/broadphase/broadphase_SSaP.h" -#include "hpp/fcl/broadphase/broadphase_interval_tree.h" -#include "hpp/fcl/broadphase/broadphase_spatialhash.h" - -#include "hpp/fcl/broadphase/default_broadphase_callbacks.h" - -#endif // ifndef HPP_FCL_BROADPHASE_BROADPHASE_H +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_SSaP.h b/include/hpp/fcl/broadphase/broadphase_SSaP.h index b99eb90c8..23dda8ef3 100644 --- a/include/hpp/fcl/broadphase/broadphase_SSaP.h +++ b/include/hpp/fcl/broadphase/broadphase_SSaP.h @@ -1,148 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROAD_PHASE_SSAP_H -#define HPP_FCL_BROAD_PHASE_SSAP_H - -#include -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" - -namespace hpp { -namespace fcl { - -/// @brief Simple SAP collision manager -class HPP_FCL_DLLAPI SSaPCollisionManager : public BroadPhaseCollisionManager { - public: - typedef BroadPhaseCollisionManager Base; - using Base::getObjects; - - SSaPCollisionManager(); - - /// @brief remove one object from the manager - void registerObject(CollisionObject* obj); - - /// @brief add one object to the manager - void unregisterObject(CollisionObject* obj); - - /// @brief initialize the manager, related with the specific type of manager - void setup(); - - /// @brief update the condition of manager - virtual void update(); - - /// @brief clear the manager - void clear(); - - /// @brief return the objects managed by the manager - void getObjects(std::vector& objs) const; - - /// @brief perform collision test between one object and all the objects - /// belonging to the manager - void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; - - /// @brief perform distance computation between one object and all the objects - /// belonging to the manager - void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; - - /// @brief perform collision test for the objects belonging to the manager - /// (i.e., N^2 self collision) - void collide(CollisionCallBackBase* callback) const; - - /// @brief perform distance test for the objects belonging to the manager - /// (i.e., N^2 self distance) - void distance(DistanceCallBackBase* callback) const; - - /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager, - CollisionCallBackBase* callback) const; - - /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager, - DistanceCallBackBase* callback) const; - - /// @brief whether the manager is empty - bool empty() const; - - /// @brief the number of objects managed by the manager - size_t size() const; - - protected: - /// @brief check collision between one object and a list of objects, return - /// value is whether stop is possible - bool checkColl( - typename std::vector::const_iterator pos_start, - typename std::vector::const_iterator pos_end, - CollisionObject* obj, CollisionCallBackBase* callback) const; - - /// @brief check distance between one object and a list of objects, return - /// value is whether stop is possible - bool checkDis( - typename std::vector::const_iterator pos_start, - typename std::vector::const_iterator pos_end, - CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const; - - bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; - - bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const; - - static int selectOptimalAxis( - const std::vector& objs_x, - const std::vector& objs_y, - const std::vector& objs_z, - typename std::vector::const_iterator& it_beg, - typename std::vector::const_iterator& it_end); - - /// @brief Objects sorted according to lower x value - std::vector objs_x; - - /// @brief Objects sorted according to lower y value - std::vector objs_y; - - /// @brief Objects sorted according to lower z value - std::vector objs_z; - - /// @brief tag about whether the environment is maintained suitably (i.e., the - /// objs_x, objs_y, objs_z are sorted correctly - bool setup_; -}; - -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_SaP.h b/include/hpp/fcl/broadphase/broadphase_SaP.h index a71f78ad4..0cdef6822 100644 --- a/include/hpp/fcl/broadphase/broadphase_SaP.h +++ b/include/hpp/fcl/broadphase/broadphase_SaP.h @@ -1,226 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROAD_PHASE_SAP_H -#define HPP_FCL_BROAD_PHASE_SAP_H - -#include -#include - -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" - -namespace hpp { -namespace fcl { - -/// @brief Rigorous SAP collision manager -class HPP_FCL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager { - public: - typedef BroadPhaseCollisionManager Base; - using Base::getObjects; - - SaPCollisionManager(); - - ~SaPCollisionManager(); - - /// @brief add objects to the manager - void registerObjects(const std::vector& other_objs); - - /// @brief remove one object from the manager - void registerObject(CollisionObject* obj); - - /// @brief add one object to the manager - void unregisterObject(CollisionObject* obj); - - /// @brief initialize the manager, related with the specific type of manager - void setup(); - - /// @brief update the condition of manager - virtual void update(); - - /// @brief update the manager by explicitly given the object updated - void update(CollisionObject* updated_obj); - - /// @brief update the manager by explicitly given the set of objects update - void update(const std::vector& updated_objs); - - /// @brief clear the manager - void clear(); - - /// @brief return the objects managed by the manager - void getObjects(std::vector& objs) const; - - /// @brief perform collision test between one object and all the objects - /// belonging to the manager - void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; - - /// @brief perform distance computation between one object and all the objects - /// belonging to the manager - void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; - - /// @brief perform collision test for the objects belonging to the manager - /// (i.e., N^2 self collision) - void collide(CollisionCallBackBase* callback) const; - - /// @brief perform distance test for the objects belonging to the manager - /// (i.e., N^2 self distance) - void distance(DistanceCallBackBase* callback) const; - - /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager, - CollisionCallBackBase* callback) const; - - /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager, - DistanceCallBackBase* callback) const; - - /// @brief whether the manager is empty - bool empty() const; - - /// @brief the number of objects managed by the manager - size_t size() const; - - protected: - struct EndPoint; - - /// @brief SAP interval for one object - struct SaPAABB { - /// @brief object - CollisionObject* obj; - - /// @brief lower bound end point of the interval - EndPoint* lo; - - /// @brief higher bound end point of the interval - EndPoint* hi; - - /// @brief cached AABB value - AABB cached; - }; - - /// @brief End point for an interval - struct EndPoint { - /// @brief tag for whether it is a lower bound or higher bound of an - /// interval, 0 for lo, and 1 for hi - char minmax; - - /// @brief back pointer to SAP interval - SaPAABB* aabb; - - /// @brief the previous end point in the end point list - EndPoint* prev[3]; - - /// @brief the next end point in the end point list - EndPoint* next[3]; - - /// @brief get the value of the end point - const Vec3f& getVal() const; - - /// @brief set the value of the end point - Vec3f& getVal(); - - FCL_REAL getVal(int i) const; - - FCL_REAL& getVal(int i); - }; - - /// @brief A pair of objects that are not culling away and should further - /// check collision - struct SaPPair { - SaPPair(CollisionObject* a, CollisionObject* b); - - CollisionObject* obj1; - CollisionObject* obj2; - - bool operator==(const SaPPair& other) const; - }; - - /// @brief Functor to help unregister one object - class HPP_FCL_DLLAPI isUnregistered { - CollisionObject* obj; - - public: - isUnregistered(CollisionObject* obj_); - - bool operator()(const SaPPair& pair) const; - }; - - /// @brief Functor to help remove collision pairs no longer valid (i.e., - /// should be culled away) - class HPP_FCL_DLLAPI isNotValidPair { - CollisionObject* obj1; - CollisionObject* obj2; - - public: - isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_); - - bool operator()(const SaPPair& pair); - }; - - void update_(SaPAABB* updated_aabb); - - void updateVelist(); - - /// @brief End point list for x, y, z coordinates - EndPoint* elist[3]; - - /// @brief vector version of elist, for acceleration - std::vector velist[3]; - - /// @brief SAP interval list - std::list AABB_arr; - - /// @brief The pair of objects that should further check for collision - std::list overlap_pairs; - - int optimal_axis; - - std::map obj_aabb_map; - - bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const; - - bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; - - void addToOverlapPairs(const SaPPair& p); - - void removeFromOverlapPairs(const SaPPair& p); -}; - -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_bruteforce.h b/include/hpp/fcl/broadphase/broadphase_bruteforce.h index 734c74fb7..5a4811ce1 100644 --- a/include/hpp/fcl/broadphase/broadphase_bruteforce.h +++ b/include/hpp/fcl/broadphase/broadphase_bruteforce.h @@ -1,115 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROAD_PHASE_BRUTE_FORCE_H -#define HPP_FCL_BROAD_PHASE_BRUTE_FORCE_H - -#include -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" - -namespace hpp { -namespace fcl { - -/// @brief Brute force N-body collision manager -class HPP_FCL_DLLAPI NaiveCollisionManager : public BroadPhaseCollisionManager { - public: - typedef BroadPhaseCollisionManager Base; - using Base::getObjects; - - NaiveCollisionManager(); - - /// @brief add objects to the manager - void registerObjects(const std::vector& other_objs); - - /// @brief add one object to the manager - void registerObject(CollisionObject* obj); - - /// @brief remove one object from the manager - void unregisterObject(CollisionObject* obj); - - /// @brief initialize the manager, related with the specific type of manager - void setup(); - - /// @brief update the condition of manager - virtual void update(); - - /// @brief clear the manager - void clear(); - - /// @brief return the objects managed by the manager - void getObjects(std::vector& objs) const; - - /// @brief perform collision test between one object and all the objects - /// belonging to the manager - void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; - - /// @brief perform distance computation between one object and all the objects - /// belonging to the manager - void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; - - /// @brief perform collision test for the objects belonging to the manager - /// (i.e., N^2 self collision) - void collide(CollisionCallBackBase* callback) const; - - /// @brief perform distance test for the objects belonging to the manager - /// (i.e., N^2 self distance) - void distance(DistanceCallBackBase* callback) const; - - /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager, - CollisionCallBackBase* callback) const; - - /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager, - DistanceCallBackBase* callback) const; - - /// @brief whether the manager is empty - bool empty() const; - - /// @brief the number of objects managed by the manager - size_t size() const; - - protected: - /// @brief objects belonging to the manager are stored in a list structure - std::list objs; -}; - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_callbacks.h b/include/hpp/fcl/broadphase/broadphase_callbacks.h index 8a7d6b0ff..40b5b0a6c 100644 --- a/include/hpp/fcl/broadphase/broadphase_callbacks.h +++ b/include/hpp/fcl/broadphase/broadphase_callbacks.h @@ -1,98 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Justin Carpentier (justin.carpentier@inria.fr) */ - -#ifndef HPP_FCL_BROADPHASE_BROAD_PHASE_CALLBACKS_H -#define HPP_FCL_BROADPHASE_BROAD_PHASE_CALLBACKS_H - -#include "hpp/fcl/fwd.hh" -#include "hpp/fcl/data_types.h" - -namespace hpp { -namespace fcl { - -/// @brief Base callback class for collision queries. -/// This class can be supersed by child classes to provide desired behaviors -/// according to the application (e.g, only listing the potential -/// CollisionObjects in collision). -struct HPP_FCL_DLLAPI CollisionCallBackBase { - /// @brief Initialization of the callback before running the collision - /// broadphase manager. - virtual void init() {}; - - /// @brief Collision evaluation between two objects in collision. - /// This callback will cause the broadphase evaluation to stop if it - /// returns true. - /// - /// @param[in] o1 Collision object #1. - /// @param[in] o2 Collision object #2. - virtual bool collide(CollisionObject* o1, CollisionObject* o2) = 0; - - /// @brief Functor call associated to the collide operation. - virtual bool operator()(CollisionObject* o1, CollisionObject* o2) { - return collide(o1, o2); - } -}; - -/// @brief Base callback class for distance queries. -/// This class can be supersed by child classes to provide desired behaviors -/// according to the application (e.g, only listing the potential -/// CollisionObjects in collision). -struct HPP_FCL_DLLAPI DistanceCallBackBase { - /// @brief Initialization of the callback before running the collision - /// broadphase manager. - virtual void init() {}; - - /// @brief Distance evaluation between two objects in collision. - /// This callback will cause the broadphase evaluation to stop if it - /// returns true. - /// - /// @param[in] o1 Collision object #1. - /// @param[in] o2 Collision object #2. - /// @param[out] dist Distance between the two collision geometries. - virtual bool distance(CollisionObject* o1, CollisionObject* o2, - FCL_REAL& dist) = 0; - - /// @brief Functor call associated to the distance operation. - virtual bool operator()(CollisionObject* o1, CollisionObject* o2, - FCL_REAL& dist) { - return distance(o1, o2, dist); - } -}; - -} // namespace fcl -} // namespace hpp - -#endif // HPP_FCL_BROADPHASE_BROAD_PHASE_CALLBACKS_H +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_collision_manager.h b/include/hpp/fcl/broadphase/broadphase_collision_manager.h index 750907b28..fe49da052 100644 --- a/include/hpp/fcl/broadphase/broadphase_collision_manager.h +++ b/include/hpp/fcl/broadphase/broadphase_collision_manager.h @@ -1,141 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H -#define HPP_FCL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H - -#include -#include -#include - -#include "hpp/fcl/collision_object.h" -#include "hpp/fcl/broadphase/broadphase_callbacks.h" - -namespace hpp { -namespace fcl { - -/// @brief Base class for broad phase collision. It helps to accelerate the -/// collision/distance between N objects. Also support self collision, self -/// distance and collision/distance with another M objects. -class HPP_FCL_DLLAPI BroadPhaseCollisionManager { - public: - BroadPhaseCollisionManager(); - - virtual ~BroadPhaseCollisionManager(); - - /// @brief add objects to the manager - virtual void registerObjects(const std::vector& other_objs); - - /// @brief add one object to the manager - virtual void registerObject(CollisionObject* obj) = 0; - - /// @brief remove one object from the manager - virtual void unregisterObject(CollisionObject* obj) = 0; - - /// @brief initialize the manager, related with the specific type of manager - virtual void setup() = 0; - - /// @brief update the condition of manager - virtual void update() = 0; - - /// @brief update the manager by explicitly given the object updated - virtual void update(CollisionObject* updated_obj); - - /// @brief update the manager by explicitly given the set of objects update - virtual void update(const std::vector& updated_objs); - - /// @brief clear the manager - virtual void clear() = 0; - - /// @brief return the objects managed by the manager - virtual void getObjects(std::vector& objs) const = 0; - - /// @brief return the objects managed by the manager - virtual std::vector getObjects() const { - std::vector res(size()); - getObjects(res); - return res; - }; - - /// @brief perform collision test between one object and all the objects - /// belonging to the manager - virtual void collide(CollisionObject* obj, - CollisionCallBackBase* callback) const = 0; - - /// @brief perform distance computation between one object and all the objects - /// belonging to the manager - virtual void distance(CollisionObject* obj, - DistanceCallBackBase* callback) const = 0; - - /// @brief perform collision test for the objects belonging to the manager - /// (i.e., N^2 self collision) - virtual void collide(CollisionCallBackBase* callback) const = 0; - - /// @brief perform distance test for the objects belonging to the manager - /// (i.e., N^2 self distance) - virtual void distance(DistanceCallBackBase* callback) const = 0; - - /// @brief perform collision test with objects belonging to another manager - virtual void collide(BroadPhaseCollisionManager* other_manager, - CollisionCallBackBase* callback) const = 0; - - /// @brief perform distance test with objects belonging to another manager - virtual void distance(BroadPhaseCollisionManager* other_manager, - DistanceCallBackBase* callback) const = 0; - - /// @brief whether the manager is empty - virtual bool empty() const = 0; - - /// @brief the number of objects managed by the manager - virtual size_t size() const = 0; - - protected: - /// @brief tools help to avoid repeating collision or distance callback for - /// the pairs of objects tested before. It can be useful for some of the - /// broadphase algorithms. - mutable std::set > tested_set; - mutable bool enable_tested_set_; - - bool inTestedSet(CollisionObject* a, CollisionObject* b) const; - - void insertTestedSet(CollisionObject* a, CollisionObject* b) const; -}; - -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h index 93691b200..5ee4f15b0 100644 --- a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h +++ b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h @@ -1,87 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H -#define HPP_FCL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H - -#include "hpp/fcl/broadphase/broadphase_continuous_collision_manager.h" -#include - -namespace hpp { -namespace fcl { - -//============================================================================== -BroadPhaseContinuousCollisionManager::BroadPhaseContinuousCollisionManager() { - // Do nothing -} - -//============================================================================== - -BroadPhaseContinuousCollisionManager::~BroadPhaseContinuousCollisionManager() { - // Do nothing -} - -//============================================================================== - -void BroadPhaseContinuousCollisionManager::registerObjects( - const std::vector& other_objs) { - for (size_t i = 0; i < other_objs.size(); ++i) registerObject(other_objs[i]); -} - -//============================================================================== - -void BroadPhaseContinuousCollisionManager::update( - ContinuousCollisionObject* updated_obj) { - HPP_FCL_UNUSED_VARIABLE(updated_obj); - - update(); -} - -//============================================================================== - -void BroadPhaseContinuousCollisionManager::update( - const std::vector& updated_objs) { - HPP_FCL_UNUSED_VARIABLE(updated_objs); - - update(); -} - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h index 79bfe6aa6..a2f73c0a1 100644 --- a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h +++ b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h @@ -1,146 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H -#define HPP_FCL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H - -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" -#include "hpp/fcl/collision_object.h" -#include "hpp/fcl/narrowphase/continuous_collision_object.h" - -namespace hpp { -namespace fcl { - -/// @brief Callback for continuous collision between two objects. Return value -/// is whether can stop now. -template -using ContinuousCollisionCallBack = bool (*)(ContinuousCollisionObject* o1, - ContinuousCollisionObject* o2, - void* cdata); - -/// @brief Callback for continuous distance between two objects, Return value is -/// whether can stop now, also return the minimum distance till now. -template -using ContinuousDistanceCallBack = bool (*)(ContinuousCollisionObject* o1, - ContinuousCollisionObject* o2, - S& dist); - -/// @brief Base class for broad phase continuous collision. It helps to -/// accelerate the continuous collision/distance between N objects. Also support -/// self collision, self distance and collision/distance with another M objects. -template -class HPP_FCL_DLLAPI BroadPhaseContinuousCollisionManager { - public: - BroadPhaseContinuousCollisionManager(); - - virtual ~BroadPhaseContinuousCollisionManager(); - - /// @brief add objects to the manager - virtual void registerObjects( - const std::vector& other_objs); - - /// @brief add one object to the manager - virtual void registerObject(ContinuousCollisionObject* obj) = 0; - - /// @brief remove one object from the manager - virtual void unregisterObject(ContinuousCollisionObject* obj) = 0; - - /// @brief initialize the manager, related with the specific type of manager - virtual void setup() = 0; - - /// @brief update the condition of manager - virtual void update() = 0; - - /// @brief update the manager by explicitly given the object updated - virtual void update(ContinuousCollisionObject* updated_obj); - - /// @brief update the manager by explicitly given the set of objects update - virtual void update( - const std::vector& updated_objs); - - /// @brief clear the manager - virtual void clear() = 0; - - /// @brief return the objects managed by the manager - virtual void getObjects( - std::vector& objs) const = 0; - - /// @brief perform collision test between one object and all the objects - /// belonging to the manager - virtual void collide(ContinuousCollisionObject* obj, - CollisionCallBackBase* callback) const = 0; - - /// @brief perform distance computation between one object and all the objects - /// belonging to the manager - virtual void distance(ContinuousCollisionObject* obj, - DistanceCallBackBase* callback) const = 0; - - /// @brief perform collision test for the objects belonging to the manager - /// (i.e., N^2 self collision) - virtual void collide(CollisionCallBackBase* callback) const = 0; - - /// @brief perform distance test for the objects belonging to the manager - /// (i.e., N^2 self distance) - virtual void distance(DistanceCallBackBase* callback) const = 0; - - /// @brief perform collision test with objects belonging to another manager - virtual void collide(BroadPhaseContinuousCollisionManager* other_manager, - CollisionCallBackBase* callback) const = 0; - - /// @brief perform distance test with objects belonging to another manager - virtual void distance(BroadPhaseContinuousCollisionManager* other_manager, - DistanceCallBackBase* callback) const = 0; - - /// @brief whether the manager is empty - virtual bool empty() const = 0; - - /// @brief the number of objects managed by the manager - virtual size_t size() const = 0; -}; - -using BroadPhaseContinuousCollisionManagerf = - BroadPhaseContinuousCollisionManager; -using BroadPhaseContinuousCollisionManagerd = - BroadPhaseContinuousCollisionManager; - -} // namespace fcl - -} // namespace hpp - -#include "hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h" - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h index 0ef013214..167ec3adc 100644 --- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h +++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h @@ -1,236 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H -#define HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H - -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" - -#include - -#if HPP_FCL_HAVE_OCTOMAP -#include "hpp/fcl/octree.h" -#endif - -#include "hpp/fcl/BV/BV.h" -#include "hpp/fcl/shape/geometric_shapes_utility.h" - -namespace hpp { -namespace fcl { -namespace detail { - -namespace dynamic_AABB_tree { - -#if HPP_FCL_HAVE_OCTOMAP - -//============================================================================== -template -bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, - const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, - const Eigen::MatrixBase& translation2, - CollisionCallBackBase* callback) { - if (!root2) { - if (root1->isLeaf()) { - CollisionObject* obj1 = static_cast(root1->data); - - if (!obj1->collisionGeometry()->isFree()) { - const AABB& root2_bv_t = translate(root2_bv, translation2); - if (root1->bv.overlap(root2_bv_t)) { - Box* box = new Box(); - Transform3f box_tf; - Transform3f tf2 = Transform3f::Identity(); - tf2.translation() = translation2; - constructBox(root2_bv, tf2, *box, box_tf); - - box->cost_density = - tree2->getOccupancyThres(); // thresholds are 0, 1, so uncertain - - CollisionObject obj2(shared_ptr(box), box_tf); - return (*callback)(obj1, &obj2); - } - } - } else { - if (collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv, - translation2, callback)) - return true; - if (collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv, - translation2, callback)) - return true; - } - - return false; - } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { - CollisionObject* obj1 = static_cast(root1->data); - - if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { - const AABB& root2_bv_t = translate(root2_bv, translation2); - if (root1->bv.overlap(root2_bv_t)) { - Box* box = new Box(); - Transform3f box_tf; - Transform3f tf2 = Transform3f::Identity(); - tf2.translation() = translation2; - constructBox(root2_bv, tf2, *box, box_tf); - - box->cost_density = root2->getOccupancy(); - box->threshold_occupied = tree2->getOccupancyThres(); - - CollisionObject obj2(shared_ptr(box), box_tf); - return (*callback)(obj1, &obj2); - } else - return false; - } else - return false; - } - - const AABB& root2_bv_t = translate(root2_bv, translation2); - if (tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false; - - if (!tree2->nodeHasChildren(root2) || - (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { - if (collisionRecurse_(root1->children[0], tree2, root2, root2_bv, - translation2, callback)) - return true; - if (collisionRecurse_(root1->children[1], tree2, root2, root2_bv, - translation2, callback)) - return true; - } else { - for (unsigned int i = 0; i < 8; ++i) { - if (tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - - if (collisionRecurse_(root1, tree2, child, child_bv, translation2, - callback)) - return true; - } else { - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - if (collisionRecurse_(root1, tree2, nullptr, child_bv, translation2, - callback)) - return true; - } - } - } - return false; -} - -//============================================================================== -template -bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, - const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, - const Eigen::MatrixBase& translation2, - DistanceCallBackBase* callback, FCL_REAL& min_dist) { - if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { - if (tree2->isNodeOccupied(root2)) { - Box* box = new Box(); - Transform3f box_tf; - Transform3f tf2 = Transform3f::Identity(); - tf2.translation() = translation2; - constructBox(root2_bv, tf2, *box, box_tf); - CollisionObject obj(shared_ptr(box), box_tf); - return (*callback)(static_cast(root1->data), &obj, - min_dist); - } else - return false; - } - - if (!tree2->isNodeOccupied(root2)) return false; - - if (!tree2->nodeHasChildren(root2) || - (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { - const AABB& aabb2 = translate(root2_bv, translation2); - FCL_REAL d1 = aabb2.distance(root1->children[0]->bv); - FCL_REAL d2 = aabb2.distance(root1->children[1]->bv); - - if (d2 < d1) { - if (d2 < min_dist) { - if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv, - translation2, callback, min_dist)) - return true; - } - - if (d1 < min_dist) { - if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv, - translation2, callback, min_dist)) - return true; - } - } else { - if (d1 < min_dist) { - if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv, - translation2, callback, min_dist)) - return true; - } - - if (d2 < min_dist) { - if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv, - translation2, callback, min_dist)) - return true; - } - } - } else { - for (unsigned int i = 0; i < 8; ++i) { - if (tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - const AABB& aabb2 = translate(child_bv, translation2); - - FCL_REAL d = root1->bv.distance(aabb2); - - if (d < min_dist) { - if (distanceRecurse_(root1, tree2, child, child_bv, translation2, - callback, min_dist)) - return true; - } - } - } - } - - return false; -} - -#endif - -} // namespace dynamic_AABB_tree -} // namespace detail -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h index 10b4341c3..5ce7c99fc 100644 --- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h +++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h @@ -1,153 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H -#define HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H - -#include -#include - -#include "hpp/fcl/fwd.hh" -// #include "hpp/fcl/BV/utility.h" -#include "hpp/fcl/shape/geometric_shapes.h" -// #include "hpp/fcl/geometry/shape/utility.h" -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" -#include "hpp/fcl/broadphase/detail/hierarchy_tree.h" - -namespace hpp { -namespace fcl { - -class HPP_FCL_DLLAPI DynamicAABBTreeCollisionManager - : public BroadPhaseCollisionManager { - public: - typedef BroadPhaseCollisionManager Base; - using Base::getObjects; - - using DynamicAABBNode = detail::NodeBase; - using DynamicAABBTable = - std::unordered_map; - - int max_tree_nonbalanced_level; - int tree_incremental_balance_pass; - int* tree_topdown_balance_threshold{nullptr}; - int* tree_topdown_level{nullptr}; - int tree_init_level; - - bool octree_as_geometry_collide; - bool octree_as_geometry_distance; - - DynamicAABBTreeCollisionManager(); - - /// @brief add objects to the manager - void registerObjects(const std::vector& other_objs); - - /// @brief add one object to the manager - void registerObject(CollisionObject* obj); - - /// @brief remove one object from the manager - void unregisterObject(CollisionObject* obj); - - /// @brief initialize the manager, related with the specific type of manager - void setup(); - - /// @brief update the condition of manager - virtual void update(); - - /// @brief update the manager by explicitly given the object updated - void update(CollisionObject* updated_obj); - - /// @brief update the manager by explicitly given the set of objects update - void update(const std::vector& updated_objs); - - /// @brief clear the manager - void clear(); - - /// @brief return the objects managed by the manager - void getObjects(std::vector& objs) const; - - /// @brief perform collision test between one object and all the objects - /// belonging to the manager - void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; - - /// @brief perform distance computation between one object and all the objects - /// belonging to the manager - void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; - - /// @brief perform collision test for the objects belonging to the manager - /// (i.e., N^2 self collision) - void collide(CollisionCallBackBase* callback) const; - - /// @brief perform distance test for the objects belonging to the manager - /// (i.e., N^2 self distance) - void distance(DistanceCallBackBase* callback) const; - - /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager_, - CollisionCallBackBase* callback) const; - - /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager_, - DistanceCallBackBase* callback) const; - - /// @brief whether the manager is empty - bool empty() const; - - /// @brief the number of objects managed by the manager - size_t size() const; - - /// @brief returns the AABB tree structure. - const detail::HierarchyTree& getTree() const; - - /// @brief returns the AABB tree structure. - detail::HierarchyTree& getTree(); - - private: - detail::HierarchyTree dtree{}; - std::unordered_map table; - - bool setup_; - - void update_(CollisionObject* updated_obj); -}; - -} // namespace fcl - -} // namespace hpp - -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h" - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h index 439f93b11..5e67f2525 100644 --- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h +++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h @@ -1,235 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H -#define HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H - -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" -#include "hpp/fcl/shape/geometric_shapes_utility.h" - -#if HPP_FCL_HAVE_OCTOMAP -#include "hpp/fcl/octree.h" -#endif - -namespace hpp { -namespace fcl { -namespace detail { -namespace dynamic_AABB_tree_array { - -#if HPP_FCL_HAVE_OCTOMAP - -//============================================================================== -template -bool collisionRecurse_( - DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, - size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Eigen::MatrixBase& translation2, - CollisionCallBackBase* callback) { - DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = - nodes1 + root1_id; - if (!root2) { - if (root1->isLeaf()) { - CollisionObject* obj1 = static_cast(root1->data); - - if (!obj1->collisionGeometry()->isFree()) { - const AABB& root_bv_t = translate(root2_bv, translation2); - if (root1->bv.overlap(root_bv_t)) { - Box* box = new Box(); - Transform3f box_tf; - Transform3f tf2 = Transform3f::Identity(); - tf2.translation() = translation2; - constructBox(root2_bv, tf2, *box, box_tf); - - box->cost_density = tree2->getDefaultOccupancy(); - - CollisionObject obj2(shared_ptr(box), box_tf); - return (*callback)(obj1, &obj2); - } - } - } else { - if (collisionRecurse_(nodes1, root1->children[0], tree2, nullptr, - root2_bv, translation2, callback)) - return true; - if (collisionRecurse_(nodes1, root1->children[1], tree2, nullptr, - root2_bv, translation2, callback)) - return true; - } - - return false; - } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { - CollisionObject* obj1 = static_cast(root1->data); - if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { - const AABB& root_bv_t = translate(root2_bv, translation2); - if (root1->bv.overlap(root_bv_t)) { - Box* box = new Box(); - Transform3f box_tf; - Transform3f tf2 = Transform3f::Identity(); - tf2.translation() = translation2; - constructBox(root2_bv, tf2, *box, box_tf); - - box->cost_density = root2->getOccupancy(); - box->threshold_occupied = tree2->getOccupancyThres(); - - CollisionObject obj2(shared_ptr(box), box_tf); - return (*callback)(obj1, &obj2); - } else - return false; - } else - return false; - } - - const AABB& root_bv_t = translate(root2_bv, translation2); - if (tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) return false; - - if (!tree2->nodeHasChildren(root2) || - (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { - if (collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, - translation2, callback)) - return true; - if (collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, - translation2, callback)) - return true; - } else { - for (unsigned int i = 0; i < 8; ++i) { - if (tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - - if (collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, - translation2, callback)) - return true; - } else { - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - if (collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv, - translation2, callback)) - return true; - } - } - } - - return false; -} - -//============================================================================== -template -bool distanceRecurse_( - DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, - size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Eigen::MatrixBase& translation2, - DistanceCallBackBase* callback, FCL_REAL& min_dist) { - DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = - nodes1 + root1_id; - if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { - if (tree2->isNodeOccupied(root2)) { - Box* box = new Box(); - Transform3f box_tf; - Transform3f tf2 = Transform3f::Identity(); - tf2.translation() = translation2; - constructBox(root2_bv, tf2, *box, box_tf); - CollisionObject obj(shared_ptr(box), box_tf); - return (*callback)(static_cast(root1->data), &obj, - min_dist); - } else - return false; - } - - if (!tree2->isNodeOccupied(root2)) return false; - - if (!tree2->nodeHasChildren(root2) || - (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { - const AABB& aabb2 = translate(root2_bv, translation2); - - FCL_REAL d1 = aabb2.distance((nodes1 + root1->children[0])->bv); - FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv); - - if (d2 < d1) { - if (d2 < min_dist) { - if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, - translation2, callback, min_dist)) - return true; - } - - if (d1 < min_dist) { - if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, - translation2, callback, min_dist)) - return true; - } - } else { - if (d1 < min_dist) { - if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, - translation2, callback, min_dist)) - return true; - } - - if (d2 < min_dist) { - if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, - translation2, callback, min_dist)) - return true; - } - } - } else { - for (unsigned int i = 0; i < 8; ++i) { - if (tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - - const AABB& aabb2 = translate(child_bv, translation2); - FCL_REAL d = root1->bv.distance(aabb2); - - if (d < min_dist) { - if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, - translation2, callback, min_dist)) - return true; - } - } - } - } - - return false; -} - -#endif - -} // namespace dynamic_AABB_tree_array -} // namespace detail -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h index e4756300b..5923fd177 100644 --- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h +++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h @@ -1,149 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H -#define HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H - -#include -#include -#include - -#include "hpp/fcl/fwd.hh" -// #include "hpp/fcl/BV/utility.h" -#include "hpp/fcl/shape/geometric_shapes.h" -// #include "hpp/fcl/geometry/shape/utility.h" -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" -#include "hpp/fcl/broadphase/detail/hierarchy_tree_array.h" - -namespace hpp { -namespace fcl { - -class HPP_FCL_DLLAPI DynamicAABBTreeArrayCollisionManager - : public BroadPhaseCollisionManager { - public: - typedef BroadPhaseCollisionManager Base; - using Base::getObjects; - - using DynamicAABBNode = detail::implementation_array::NodeBase; - using DynamicAABBTable = std::unordered_map; - - int max_tree_nonbalanced_level; - int tree_incremental_balance_pass; - int* tree_topdown_balance_threshold{nullptr}; - int* tree_topdown_level{nullptr}; - int tree_init_level; - - bool octree_as_geometry_collide; - bool octree_as_geometry_distance; - - DynamicAABBTreeArrayCollisionManager(); - - /// @brief add objects to the manager - void registerObjects(const std::vector& other_objs); - - /// @brief add one object to the manager - void registerObject(CollisionObject* obj); - - /// @brief remove one object from the manager - void unregisterObject(CollisionObject* obj); - - /// @brief initialize the manager, related with the specific type of manager - void setup(); - - /// @brief update the condition of manager - virtual void update(); - - /// @brief update the manager by explicitly given the object updated - void update(CollisionObject* updated_obj); - - /// @brief update the manager by explicitly given the set of objects update - void update(const std::vector& updated_objs); - - /// @brief clear the manager - void clear(); - - /// @brief return the objects managed by the manager - void getObjects(std::vector& objs) const; - - /// @brief perform collision test between one object and all the objects - /// belonging to the manager - void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; - - /// @brief perform distance computation between one object and all the objects - /// belonging to the manager - void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; - - /// @brief perform collision test for the objects belonging to the manager - /// (i.e., N^2 self collision) - void collide(CollisionCallBackBase* callback) const; - - /// @brief perform distance test for the objects belonging to the manager - /// (i.e., N^2 self distance) - void distance(DistanceCallBackBase* callback) const; - - /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager_, - CollisionCallBackBase* callback) const; - - /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager_, - DistanceCallBackBase* callback) const; - - /// @brief whether the manager is empty - bool empty() const; - - /// @brief the number of objects managed by the manager - size_t size() const; - - const detail::implementation_array::HierarchyTree& getTree() const; - - private: - detail::implementation_array::HierarchyTree dtree{}; - std::unordered_map table; - - bool setup_; - - void update_(CollisionObject* updated_obj); -}; - -} // namespace fcl - -} // namespace hpp - -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h" - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_interval_tree.h b/include/hpp/fcl/broadphase/broadphase_interval_tree.h index 78b1037d0..d00b1f6d3 100644 --- a/include/hpp/fcl/broadphase/broadphase_interval_tree.h +++ b/include/hpp/fcl/broadphase/broadphase_interval_tree.h @@ -1,172 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROAD_PHASE_INTERVAL_TREE_H -#define HPP_FCL_BROAD_PHASE_INTERVAL_TREE_H - -#include -#include - -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" -#include "hpp/fcl/broadphase/detail/interval_tree.h" - -namespace hpp { -namespace fcl { - -/// @brief Collision manager based on interval tree -class HPP_FCL_DLLAPI IntervalTreeCollisionManager - : public BroadPhaseCollisionManager { - public: - typedef BroadPhaseCollisionManager Base; - using Base::getObjects; - - IntervalTreeCollisionManager(); - - ~IntervalTreeCollisionManager(); - - /// @brief remove one object from the manager - void registerObject(CollisionObject* obj); - - /// @brief add one object to the manager - void unregisterObject(CollisionObject* obj); - - /// @brief initialize the manager, related with the specific type of manager - void setup(); - - /// @brief update the condition of manager - virtual void update(); - - /// @brief update the manager by explicitly given the object updated - void update(CollisionObject* updated_obj); - - /// @brief update the manager by explicitly given the set of objects update - void update(const std::vector& updated_objs); - - /// @brief clear the manager - void clear(); - - /// @brief return the objects managed by the manager - void getObjects(std::vector& objs) const; - - /// @brief perform collision test between one object and all the objects - /// belonging to the manager - void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; - - /// @brief perform distance computation between one object and all the objects - /// belonging to the manager - void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; - - /// @brief perform collision test for the objects belonging to the manager - /// (i.e., N^2 self collision) - void collide(CollisionCallBackBase* callback) const; - - /// @brief perform distance test for the objects belonging to the manager - /// (i.e., N^2 self distance) - void distance(DistanceCallBackBase* callback) const; - - /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager, - CollisionCallBackBase* callback) const; - - /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager, - DistanceCallBackBase* callback) const; - - /// @brief whether the manager is empty - bool empty() const; - - /// @brief the number of objects managed by the manager - size_t size() const; - - protected: - /// @brief SAP end point - /// @brief SAP end point - struct HPP_FCL_DLLAPI EndPoint { - /// @brief object related with the end point - CollisionObject* obj; - - /// @brief end point value - FCL_REAL value; - - /// @brief tag for whether it is a lower bound or higher bound of an - /// interval, 0 for lo, and 1 for hi - char minmax; - - bool operator<(const EndPoint& p) const; - }; - - /// @brief Extention interval tree's interval to SAP interval, adding more - /// information - struct HPP_FCL_DLLAPI SAPInterval : public detail::SimpleInterval { - CollisionObject* obj; - - SAPInterval(FCL_REAL low_, FCL_REAL high_, CollisionObject* obj_); - }; - - bool checkColl( - typename std::deque::const_iterator pos_start, - typename std::deque::const_iterator pos_end, - CollisionObject* obj, CollisionCallBackBase* callback) const; - - bool checkDist( - typename std::deque::const_iterator pos_start, - typename std::deque::const_iterator pos_end, - CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const; - - bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; - - bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const; - - /// @brief vector stores all the end points - std::vector endpoints[3]; - - /// @brief interval tree manages the intervals - detail::IntervalTree* interval_trees[3]; - - std::map obj_interval_maps[3]; - - /// @brief tag for whether the interval tree is maintained suitably - bool setup_; -}; - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h b/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h index 29c462c3e..31917d0a6 100644 --- a/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h +++ b/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h @@ -1,542 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H -#define HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H - -#include "hpp/fcl/broadphase/broadphase_spatialhash.h" - -namespace hpp { -namespace fcl { - -//============================================================================== -template -SpatialHashingCollisionManager::SpatialHashingCollisionManager( - FCL_REAL cell_size, const Vec3f& scene_min, const Vec3f& scene_max, - unsigned int default_table_size) - : scene_limit(AABB(scene_min, scene_max)), - hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) { - hash_table->init(default_table_size); -} - -//============================================================================== -template -SpatialHashingCollisionManager::~SpatialHashingCollisionManager() { - delete hash_table; -} - -//============================================================================== -template -void SpatialHashingCollisionManager::registerObject( - CollisionObject* obj) { - objs.push_back(obj); - - const AABB& obj_aabb = obj->getAABB(); - AABB overlap_aabb; - - if (scene_limit.overlap(obj_aabb, overlap_aabb)) { - if (!scene_limit.contain(obj_aabb)) - objs_partially_penetrating_scene_limit.push_back(obj); - - hash_table->insert(overlap_aabb, obj); - } else { - objs_outside_scene_limit.push_back(obj); - } - - obj_aabb_map[obj] = obj_aabb; -} - -//============================================================================== -template -void SpatialHashingCollisionManager::unregisterObject( - CollisionObject* obj) { - objs.remove(obj); - - const AABB& obj_aabb = obj->getAABB(); - AABB overlap_aabb; - - if (scene_limit.overlap(obj_aabb, overlap_aabb)) { - if (!scene_limit.contain(obj_aabb)) - objs_partially_penetrating_scene_limit.remove(obj); - - hash_table->remove(overlap_aabb, obj); - } else { - objs_outside_scene_limit.remove(obj); - } - - obj_aabb_map.erase(obj); -} - -//============================================================================== -template -void SpatialHashingCollisionManager::setup() { - // Do nothing -} - -//============================================================================== -template -void SpatialHashingCollisionManager::update() { - hash_table->clear(); - objs_partially_penetrating_scene_limit.clear(); - objs_outside_scene_limit.clear(); - - for (auto it = objs.cbegin(), end = objs.cend(); it != end; ++it) { - CollisionObject* obj = *it; - const AABB& obj_aabb = obj->getAABB(); - AABB overlap_aabb; - - if (scene_limit.overlap(obj_aabb, overlap_aabb)) { - if (!scene_limit.contain(obj_aabb)) - objs_partially_penetrating_scene_limit.push_back(obj); - - hash_table->insert(overlap_aabb, obj); - } else { - objs_outside_scene_limit.push_back(obj); - } - - obj_aabb_map[obj] = obj_aabb; - } -} - -//============================================================================== -template -void SpatialHashingCollisionManager::update( - CollisionObject* updated_obj) { - const AABB& new_aabb = updated_obj->getAABB(); - const AABB& old_aabb = obj_aabb_map[updated_obj]; - - AABB old_overlap_aabb; - const auto is_old_aabb_overlapping = - scene_limit.overlap(old_aabb, old_overlap_aabb); - if (is_old_aabb_overlapping) - hash_table->remove(old_overlap_aabb, updated_obj); - - AABB new_overlap_aabb; - const auto is_new_aabb_overlapping = - scene_limit.overlap(new_aabb, new_overlap_aabb); - if (is_new_aabb_overlapping) - hash_table->insert(new_overlap_aabb, updated_obj); - - ObjectStatus old_status; - if (is_old_aabb_overlapping) { - if (scene_limit.contain(old_aabb)) - old_status = Inside; - else - old_status = PartiallyPenetrating; - } else { - old_status = Outside; - } - - if (is_new_aabb_overlapping) { - if (scene_limit.contain(new_aabb)) { - if (old_status == PartiallyPenetrating) { - // Status change: PartiallyPenetrating --> Inside - // Required action(s): - // - remove object from "objs_partially_penetrating_scene_limit" - - auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(), - objs_partially_penetrating_scene_limit.end(), - updated_obj); - objs_partially_penetrating_scene_limit.erase(find_it); - } else if (old_status == Outside) { - // Status change: Outside --> Inside - // Required action(s): - // - remove object from "objs_outside_scene_limit" - - auto find_it = std::find(objs_outside_scene_limit.begin(), - objs_outside_scene_limit.end(), updated_obj); - objs_outside_scene_limit.erase(find_it); - } - } else { - if (old_status == Inside) { - // Status change: Inside --> PartiallyPenetrating - // Required action(s): - // - add object to "objs_partially_penetrating_scene_limit" - - objs_partially_penetrating_scene_limit.push_back(updated_obj); - } else if (old_status == Outside) { - // Status change: Outside --> PartiallyPenetrating - // Required action(s): - // - remove object from "objs_outside_scene_limit" - // - add object to "objs_partially_penetrating_scene_limit" - - auto find_it = std::find(objs_outside_scene_limit.begin(), - objs_outside_scene_limit.end(), updated_obj); - objs_outside_scene_limit.erase(find_it); - - objs_partially_penetrating_scene_limit.push_back(updated_obj); - } - } - } else { - if (old_status == Inside) { - // Status change: Inside --> Outside - // Required action(s): - // - add object to "objs_outside_scene_limit" - - objs_outside_scene_limit.push_back(updated_obj); - } else if (old_status == PartiallyPenetrating) { - // Status change: PartiallyPenetrating --> Outside - // Required action(s): - // - remove object from "objs_partially_penetrating_scene_limit" - // - add object to "objs_outside_scene_limit" - - auto find_it = - std::find(objs_partially_penetrating_scene_limit.begin(), - objs_partially_penetrating_scene_limit.end(), updated_obj); - objs_partially_penetrating_scene_limit.erase(find_it); - - objs_outside_scene_limit.push_back(updated_obj); - } - } - - obj_aabb_map[updated_obj] = new_aabb; -} - -//============================================================================== -template -void SpatialHashingCollisionManager::update( - const std::vector& updated_objs) { - for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]); -} - -//============================================================================== -template -void SpatialHashingCollisionManager::clear() { - objs.clear(); - hash_table->clear(); - objs_outside_scene_limit.clear(); - obj_aabb_map.clear(); -} - -//============================================================================== -template -void SpatialHashingCollisionManager::getObjects( - std::vector& objs_) const { - objs_.resize(objs.size()); - std::copy(objs.begin(), objs.end(), objs_.begin()); -} - -//============================================================================== -template -void SpatialHashingCollisionManager::collide( - CollisionObject* obj, CollisionCallBackBase* callback) const { - if (size() == 0) return; - collide_(obj, callback); -} - -//============================================================================== -template -void SpatialHashingCollisionManager::distance( - CollisionObject* obj, DistanceCallBackBase* callback) const { - if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); - distance_(obj, callback, min_dist); -} - -//============================================================================== -template -bool SpatialHashingCollisionManager::collide_( - CollisionObject* obj, CollisionCallBackBase* callback) const { - const auto& obj_aabb = obj->getAABB(); - AABB overlap_aabb; - - if (scene_limit.overlap(obj_aabb, overlap_aabb)) { - const auto query_result = hash_table->query(overlap_aabb); - for (const auto& obj2 : query_result) { - if (obj == obj2) continue; - - if ((*callback)(obj, obj2)) return true; - } - - if (!scene_limit.contain(obj_aabb)) { - for (const auto& obj2 : objs_outside_scene_limit) { - if (obj == obj2) continue; - - if ((*callback)(obj, obj2)) return true; - } - } - } else { - for (const auto& obj2 : objs_partially_penetrating_scene_limit) { - if (obj == obj2) continue; - - if ((*callback)(obj, obj2)) return true; - } - - for (const auto& obj2 : objs_outside_scene_limit) { - if (obj == obj2) continue; - - if ((*callback)(obj, obj2)) return true; - } - } - - return false; -} - -//============================================================================== -template -bool SpatialHashingCollisionManager::distance_( - CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const { - auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; - auto aabb = obj->getAABB(); - if (min_dist < (std::numeric_limits::max)()) { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); - aabb.expand(min_dist_delta); - } - - AABB overlap_aabb; - - auto status = 1; - FCL_REAL old_min_distance; - - while (1) { - old_min_distance = min_dist; - - if (scene_limit.overlap(aabb, overlap_aabb)) { - if (distanceObjectToObjects(obj, hash_table->query(overlap_aabb), - callback, min_dist)) { - return true; - } - - if (!scene_limit.contain(aabb)) { - if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback, - min_dist)) { - return true; - } - } - } else { - if (distanceObjectToObjects(obj, objs_partially_penetrating_scene_limit, - callback, min_dist)) { - return true; - } - - if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback, - min_dist)) { - return true; - } - } - - if (status == 1) { - if (old_min_distance < (std::numeric_limits::max)()) { - break; - } else { - if (min_dist < old_min_distance) { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); - aabb = AABB(obj->getAABB(), min_dist_delta); - status = 0; - } else { - if (aabb == obj->getAABB()) - aabb.expand(delta); - else - aabb.expand(obj->getAABB(), 2.0); - } - } - } else if (status == 0) { - break; - } - } - - return false; -} - -//============================================================================== -template -void SpatialHashingCollisionManager::collide( - CollisionCallBackBase* callback) const { - if (size() == 0) return; - - for (const auto& obj1 : objs) { - const auto& obj_aabb = obj1->getAABB(); - AABB overlap_aabb; - - if (scene_limit.overlap(obj_aabb, overlap_aabb)) { - auto query_result = hash_table->query(overlap_aabb); - for (const auto& obj2 : query_result) { - if (obj1 < obj2) { - if ((*callback)(obj1, obj2)) return; - } - } - - if (!scene_limit.contain(obj_aabb)) { - for (const auto& obj2 : objs_outside_scene_limit) { - if (obj1 < obj2) { - if ((*callback)(obj1, obj2)) return; - } - } - } - } else { - for (const auto& obj2 : objs_partially_penetrating_scene_limit) { - if (obj1 < obj2) { - if ((*callback)(obj1, obj2)) return; - } - } - - for (const auto& obj2 : objs_outside_scene_limit) { - if (obj1 < obj2) { - if ((*callback)(obj1, obj2)) return; - } - } - } - } -} - -//============================================================================== -template -void SpatialHashingCollisionManager::distance( - DistanceCallBackBase* callback) const { - if (size() == 0) return; - - this->enable_tested_set_ = true; - this->tested_set.clear(); - - FCL_REAL min_dist = (std::numeric_limits::max)(); - - for (const auto& obj : objs) { - if (distance_(obj, callback, min_dist)) break; - } - - this->enable_tested_set_ = false; - this->tested_set.clear(); -} - -//============================================================================== -template -void SpatialHashingCollisionManager::collide( - BroadPhaseCollisionManager* other_manager_, - CollisionCallBackBase* callback) const { - auto* other_manager = - static_cast*>(other_manager_); - - if ((size() == 0) || (other_manager->size() == 0)) return; - - if (this == other_manager) { - collide(callback); - return; - } - - if (this->size() < other_manager->size()) { - for (const auto& obj : objs) { - if (other_manager->collide_(obj, callback)) return; - } - } else { - for (const auto& obj : other_manager->objs) { - if (collide_(obj, callback)) return; - } - } -} - -//============================================================================== -template -void SpatialHashingCollisionManager::distance( - BroadPhaseCollisionManager* other_manager_, - DistanceCallBackBase* callback) const { - auto* other_manager = - static_cast*>(other_manager_); - - if ((size() == 0) || (other_manager->size() == 0)) return; - - if (this == other_manager) { - distance(callback); - return; - } - - FCL_REAL min_dist = (std::numeric_limits::max)(); - - if (this->size() < other_manager->size()) { - for (const auto& obj : objs) - if (other_manager->distance_(obj, callback, min_dist)) return; - } else { - for (const auto& obj : other_manager->objs) - if (distance_(obj, callback, min_dist)) return; - } -} - -//============================================================================== -template -bool SpatialHashingCollisionManager::empty() const { - return objs.empty(); -} - -//============================================================================== -template -size_t SpatialHashingCollisionManager::size() const { - return objs.size(); -} - -//============================================================================== -template -void SpatialHashingCollisionManager::computeBound( - std::vector& objs, Vec3f& l, Vec3f& u) { - AABB bound; - for (unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB(); - - l = bound.min_; - u = bound.max_; -} - -//============================================================================== -template -template -bool SpatialHashingCollisionManager::distanceObjectToObjects( - CollisionObject* obj, const Container& objs, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const { - for (auto& obj2 : objs) { - if (obj == obj2) continue; - - if (!this->enable_tested_set_) { - if (obj->getAABB().distance(obj2->getAABB()) < min_dist) { - if ((*callback)(obj, obj2, min_dist)) return true; - } - } else { - if (!this->inTestedSet(obj, obj2)) { - if (obj->getAABB().distance(obj2->getAABB()) < min_dist) { - if ((*callback)(obj, obj2, min_dist)) return true; - } - - this->insertTestedSet(obj, obj2); - } - } - } - - return false; -} - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_spatialhash.h b/include/hpp/fcl/broadphase/broadphase_spatialhash.h index 606b82ada..5ad526077 100644 --- a/include/hpp/fcl/broadphase/broadphase_spatialhash.h +++ b/include/hpp/fcl/broadphase/broadphase_spatialhash.h @@ -1,170 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_H -#define HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_H - -#include -#include -#include "hpp/fcl/BV/AABB.h" -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" -#include "hpp/fcl/broadphase/detail/simple_hash_table.h" -#include "hpp/fcl/broadphase/detail/sparse_hash_table.h" -#include "hpp/fcl/broadphase/detail/spatial_hash.h" - -namespace hpp { -namespace fcl { - -/// @brief spatial hashing collision mananger -template > -class SpatialHashingCollisionManager : public BroadPhaseCollisionManager { - public: - typedef BroadPhaseCollisionManager Base; - using Base::getObjects; - - SpatialHashingCollisionManager(FCL_REAL cell_size, const Vec3f& scene_min, - const Vec3f& scene_max, - unsigned int default_table_size = 1000); - - ~SpatialHashingCollisionManager(); - - /// @brief add one object to the manager - void registerObject(CollisionObject* obj); - - /// @brief remove one object from the manager - void unregisterObject(CollisionObject* obj); - - /// @brief initialize the manager, related with the specific type of manager - void setup(); - - /// @brief update the condition of manager - virtual void update(); - - /// @brief update the manager by explicitly given the object updated - void update(CollisionObject* updated_obj); - - /// @brief update the manager by explicitly given the set of objects update - void update(const std::vector& updated_objs); - - /// @brief clear the manager - void clear(); - - /// @brief return the objects managed by the manager - void getObjects(std::vector& objs) const; - - /// @brief perform collision test between one object and all the objects - /// belonging to the manager - void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; - - /// @brief perform distance computation between one object and all the objects - /// belonging ot the manager - void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; - - /// @brief perform collision test for the objects belonging to the manager - /// (i.e, N^2 self collision) - void collide(CollisionCallBackBase* callback) const; - - /// @brief perform distance test for the objects belonging to the manager - /// (i.e., N^2 self distance) - void distance(DistanceCallBackBase* callback) const; - - /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager, - CollisionCallBackBase* callback) const; - - /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager, - DistanceCallBackBase* callback) const; - - /// @brief whether the manager is empty - bool empty() const; - - /// @brief the number of objects managed by the manager - size_t size() const; - - /// @brief compute the bound for the environent - static void computeBound(std::vector& objs, Vec3f& l, - Vec3f& u); - - protected: - /// @brief perform collision test between one object and all the objects - /// belonging to the manager - bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; - - /// @brief perform distance computation between one object and all the objects - /// belonging ot the manager - bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const; - - /// @brief all objects in the scene - std::list objs; - - /// @brief objects partially penetrating (not totally inside nor outside) the - /// scene limit are in another list - std::list objs_partially_penetrating_scene_limit; - - /// @brief objects outside the scene limit are in another list - std::list objs_outside_scene_limit; - - /// @brief the size of the scene - AABB scene_limit; - - /// @brief store the map between objects and their aabbs. will make update - /// more convenient - std::map obj_aabb_map; - - /// @brief objects in the scene limit (given by scene_min and scene_max) are - /// in the spatial hash table - HashTable* hash_table; - - private: - enum ObjectStatus { Inside, PartiallyPenetrating, Outside }; - - template - bool distanceObjectToObjects(CollisionObject* obj, const Container& objs, - DistanceCallBackBase* callback, - FCL_REAL& min_dist) const; -}; - -} // namespace fcl - -} // namespace hpp - -#include "hpp/fcl/broadphase/broadphase_spatialhash-inl.h" - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/default_broadphase_callbacks.h b/include/hpp/fcl/broadphase/default_broadphase_callbacks.h index b984226da..7d78f9532 100644 --- a/include/hpp/fcl/broadphase/default_broadphase_callbacks.h +++ b/include/hpp/fcl/broadphase/default_broadphase_callbacks.h @@ -1,258 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, Toyota Research Institute - * Copyright (c) 2022-2023, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Sean Curtis (sean@tri.global) */ -/** @author Justin Carpentier (justin.carpentier@inria.fr) */ - -#ifndef HPP_FCL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H -#define HPP_FCL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H - -#include "hpp/fcl/broadphase/broadphase_callbacks.h" -#include "hpp/fcl/collision.h" -#include "hpp/fcl/distance.h" -// #include "hpp/fcl/narrowphase/continuous_collision.h" -// #include "hpp/fcl/narrowphase/continuous_collision_request.h" -// #include "hpp/fcl/narrowphase/continuous_collision_result.h" -// #include "hpp/fcl/narrowphase/distance_request.h" -// #include "hpp/fcl/narrowphase/distance_result.h" - -namespace hpp { -namespace fcl { - -/// @brief Collision data stores the collision request and the result given by -/// collision algorithm. -struct CollisionData { - CollisionData() { done = false; } - - /// @brief Collision request - CollisionRequest request; - - /// @brief Collision result - CollisionResult result; - - /// @brief Whether the collision iteration can stop - bool done; - - /// @brief Clears the CollisionData - void clear() { - result.clear(); - done = false; - } -}; - -/// @brief Distance data stores the distance request and the result given by -/// distance algorithm. -struct DistanceData { - DistanceData() { done = false; } - - /// @brief Distance request - DistanceRequest request; - - /// @brief Distance result - DistanceResult result; - - /// @brief Whether the distance iteration can stop - bool done; - - /// @brief Clears the DistanceData - void clear() { - result.clear(); - done = false; - } -}; - -/// @brief Provides a simple callback for the collision query in the -/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and -/// points to an instance of CollisionData. It simply invokes the -/// `collide()` method on the culled pair of geometries and stores the results -/// in the data's CollisionResult instance. -/// -/// This callback will cause the broadphase evaluation to stop if: -/// - the collision requests _disables_ cost _and_ -/// - the collide() reports a collision for the culled pair, _and_ -/// - we've reported the number of contacts requested in the CollisionRequest. -/// -/// For a given instance of CollisionData, if broadphase evaluation has -/// already terminated (i.e., defaultCollisionFunction() returned `true`), -/// subsequent invocations with the same instance of CollisionData will -/// return immediately, requesting termination of broadphase evaluation (i.e., -/// return `true`). -/// -/// @param o1 The first object in the culled pair. -/// @param o2 The second object in the culled pair. -/// @param data A non-null pointer to a CollisionData instance. -/// @return `true` if the broadphase evaluation should stop. -/// @tparam S The scalar type with which the computation will be performed. -bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, - void* data); - -/// @brief Collision data for use with the DefaultContinuousCollisionFunction. -/// It stores the collision request and the result given by the collision -/// algorithm (and stores the conclusion of whether further evaluation of the -/// broadphase collision manager has been deemed unnecessary). -// struct DefaultContinuousCollisionData { -// ContinuousCollisionRequest request; -// ContinuousCollisionResult result; -// -// /// If `true`, requests that the broadphase evaluation stop. -// bool done{false}; -// }; - -/// @brief Provides a simple callback for the continuous collision query in the -/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and -/// points to an instance of DefaultContinuousCollisionData. It simply invokes -/// the `collide()` method on the culled pair of geometries and stores the -/// results in the data's ContinuousCollisionResult instance. -/// -/// This callback will never cause the broadphase evaluation to terminate early. -/// However, if the `done` member of the DefaultContinuousCollisionData is set -/// to true, this method will simply return without doing any computation. -/// -/// For a given instance of DefaultContinuousCollisionData, if broadphase -/// evaluation has already terminated (i.e., -/// DefaultContinuousCollisionFunction() returned `true`), subsequent -/// invocations with the same instance of CollisionData will return -/// immediately, requesting termination of broadphase evaluation (i.e., return -/// `true`). -/// -/// @param o1 The first object in the culled pair. -/// @param o2 The second object in the culled pair. -/// @param data A non-null pointer to a CollisionData instance. -/// @return True if the broadphase evaluation should stop. -/// @tparam S The scalar type with which the computation will be performed. -// bool DefaultContinuousCollisionFunction(ContinuousCollisionObject* o1, -// ContinuousCollisionObject* o2, -// void* data) { -// assert(data != nullptr); -// auto* cdata = static_cast(data); -// -// if (cdata->done) return true; -// -// const ContinuousCollisionRequest& request = cdata->request; -// ContinuousCollisionResult& result = cdata->result; -// collide(o1, o2, request, result); -// -// return cdata->done; -// } - -/// @brief Provides a simple callback for the distance query in the -/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and -/// points to an instance of DistanceData. It simply invokes the -/// `distance()` method on the culled pair of geometries and stores the results -/// in the data's DistanceResult instance. -/// -/// This callback will cause the broadphase evaluation to stop if: -/// - The distance is less than or equal to zero (i.e., the pair is in -/// contact). -/// -/// For a given instance of DistanceData, if broadphase evaluation has -/// already terminated (i.e., defaultDistanceFunction() returned `true`), -/// subsequent invocations with the same instance of DistanceData will -/// simply report the previously reported minimum distance and request -/// termination of broadphase evaluation (i.e., return `true`). -/// -/// @param o1 The first object in the culled pair. -/// @param o2 The second object in the culled pair. -/// @param data A non-null pointer to a DistanceData instance. -/// @param dist The distance computed by distance(). -/// @return `true` if the broadphase evaluation should stop. -/// @tparam S The scalar type with which the computation will be performed. -bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, - void* data, FCL_REAL& dist); - -/// @brief Default collision callback to check collision between collision -/// objects. -struct HPP_FCL_DLLAPI CollisionCallBackDefault : CollisionCallBackBase { - /// @brief Initialize the callback. - /// Clears the collision result and sets the done boolean to false. - void init() { data.clear(); } - - bool collide(CollisionObject* o1, CollisionObject* o2); - - CollisionData data; - - virtual ~CollisionCallBackDefault() {}; -}; - -/// @brief Default distance callback to check collision between collision -/// objects. -struct HPP_FCL_DLLAPI DistanceCallBackDefault : DistanceCallBackBase { - /// @brief Initialize the callback. - /// Clears the distance result and sets the done boolean to false. - void init() { data.clear(); } - - bool distance(CollisionObject* o1, CollisionObject* o2, FCL_REAL& dist); - - DistanceData data; - - virtual ~DistanceCallBackDefault() {}; -}; - -/// @brief Collision callback to collect collision pairs potentially in contacts -struct HPP_FCL_DLLAPI CollisionCallBackCollect : CollisionCallBackBase { - typedef std::pair CollisionPair; - - /// @brief Default constructor. - CollisionCallBackCollect(const size_t max_size); - - bool collide(CollisionObject* o1, CollisionObject* o2); - - /// @brief Returns the number of registered collision pairs - size_t numCollisionPairs() const; - - /// @brief Returns a const reference to the active collision_pairs to check - const std::vector& getCollisionPairs() const; - - /// @brief Reset the callback - void init(); - - /// @brief Check whether a collision pair exists - bool exist(const CollisionPair& pair) const; - - /// @brief Check whether a collision pair exists - bool exist(CollisionObject* o1, CollisionObject* o2) const; - - virtual ~CollisionCallBackCollect() {}; - - protected: - std::vector collision_pairs; - size_t max_size; -}; - -} // namespace fcl - -} // namespace hpp - -#endif // HPP_FCL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h index 36d8df358..54b709d79 100644 --- a/include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h +++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h @@ -1,1000 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_HIERARCHY_TREE_INL_H -#define HPP_FCL_HIERARCHY_TREE_INL_H - -#include "hpp/fcl/broadphase/detail/hierarchy_tree.h" - -namespace hpp { -namespace fcl { - -namespace detail { - -//============================================================================== -template -HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) { - root_node = nullptr; - n_leaves = 0; - free_node = nullptr; - max_lookahead_level = -1; - opath = 0; - bu_threshold = bu_threshold_; - topdown_level = topdown_level_; -} - -//============================================================================== -template -HierarchyTree::~HierarchyTree() { - clear(); -} - -//============================================================================== -template -void HierarchyTree::init(std::vector& leaves, int level) { - switch (level) { - case 0: - init_0(leaves); - break; - case 1: - init_1(leaves); - break; - case 2: - init_2(leaves); - break; - case 3: - init_3(leaves); - break; - default: - init_0(leaves); - } -} - -//============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::insert(const BV& bv, - void* data) { - Node* leaf = createNode(nullptr, bv, data); - insertLeaf(root_node, leaf); - ++n_leaves; - return leaf; -} - -//============================================================================== -template -void HierarchyTree::remove(Node* leaf) { - removeLeaf(leaf); - deleteNode(leaf); - --n_leaves; -} - -//============================================================================== -template -void HierarchyTree::clear() { - if (root_node) recurseDeleteNode(root_node); - n_leaves = 0; - delete free_node; - free_node = nullptr; - max_lookahead_level = -1; - opath = 0; -} - -//============================================================================== -template -bool HierarchyTree::empty() const { - return (nullptr == root_node); -} - -//============================================================================== -template -void HierarchyTree::update(Node* leaf, int lookahead_level) { - // TODO(DamrongGuoy): Since we update a leaf node by removing and - // inserting the same leaf node, it is likely to change the structure of - // the tree even if no object's pose has changed. In the future, - // find a way to preserve the structure of the tree to solve this issue: - // https://github.com/flexible-collision-library/fcl/issues/368 - - // First we remove the leaf node pointed by `leaf` variable from the tree. - // The `sub_root` variable is the root of the subtree containing nodes - // whose BVs were adjusted due to node removal. - typename HierarchyTree::Node* sub_root = removeLeaf(leaf); - if (sub_root) { - if (lookahead_level > 0) { - // For positive `lookahead_level`, we move the `sub_root` variable up. - for (int i = 0; (i < lookahead_level) && sub_root->parent; ++i) - sub_root = sub_root->parent; - } else - // By default, lookahead_level = -1, and we reset the `sub_root` variable - // to the root of the entire tree. - sub_root = root_node; - } - // Then we insert the node pointed by `leaf` variable back into the - // subtree rooted at `sub_root` variable. - insertLeaf(sub_root, leaf); -} - -//============================================================================== -template -bool HierarchyTree::update(Node* leaf, const BV& bv) { - if (leaf->bv.contain(bv)) return false; - update_(leaf, bv); - return true; -} - -//============================================================================== -template -struct UpdateImpl { - static bool run(const HierarchyTree& tree, - typename HierarchyTree::Node* leaf, const BV& bv, - const Vec3f& /*vel*/, FCL_REAL /*margin*/) { - if (leaf->bv.contain(bv)) return false; - tree.update_(leaf, bv); - return true; - } - - static bool run(const HierarchyTree& tree, - typename HierarchyTree::Node* leaf, const BV& bv, - const Vec3f& /*vel*/) { - if (leaf->bv.contain(bv)) return false; - tree.update_(leaf, bv); - return true; - } -}; - -//============================================================================== -template -bool HierarchyTree::update(Node* leaf, const BV& bv, const Vec3f& vel, - FCL_REAL margin) { - return UpdateImpl::run(*this, leaf, bv, vel, margin); -} - -//============================================================================== -template -bool HierarchyTree::update(Node* leaf, const BV& bv, const Vec3f& vel) { - return UpdateImpl::run(*this, leaf, bv, vel); -} - -//============================================================================== -template -size_t HierarchyTree::getMaxHeight() const { - if (!root_node) return 0; - return getMaxHeight(root_node); -} - -//============================================================================== -template -size_t HierarchyTree::getMaxDepth() const { - if (!root_node) return 0; - - size_t max_depth; - getMaxDepth(root_node, 0, max_depth); - return max_depth; -} - -//============================================================================== -template -void HierarchyTree::balanceBottomup() { - if (root_node) { - std::vector leaves; - leaves.reserve(n_leaves); - fetchLeaves(root_node, leaves); - bottomup(leaves.begin(), leaves.end()); - root_node = leaves[0]; - } -} - -//============================================================================== -template -void HierarchyTree::balanceTopdown() { - if (root_node) { - std::vector leaves; - leaves.reserve(n_leaves); - fetchLeaves(root_node, leaves); - root_node = topdown(leaves.begin(), leaves.end()); - } -} - -//============================================================================== -template -void HierarchyTree::balanceIncremental(int iterations) { - if (iterations < 0) iterations = (int)n_leaves; - if (root_node && (iterations > 0)) { - for (int i = 0; i < iterations; ++i) { - Node* node = root_node; - unsigned int bit = 0; - while (!node->isLeaf()) { - node = sort(node, root_node)->children[(opath >> bit) & 1]; - bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1); - } - update(node); - ++opath; - } - } -} - -//============================================================================== -template -void HierarchyTree::refit() { - if (root_node) recurseRefit(root_node); -} - -//============================================================================== -template -void HierarchyTree::extractLeaves(const Node* root, - std::vector& leaves) const { - if (!root->isLeaf()) { - extractLeaves(root->children[0], leaves); - extractLeaves(root->children[1], leaves); - } else - leaves.push_back(root); -} - -//============================================================================== -template -size_t HierarchyTree::size() const { - return n_leaves; -} - -//============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::getRoot() const { - return root_node; -} - -//============================================================================== -template -typename HierarchyTree::Node*& HierarchyTree::getRoot() { - return root_node; -} - -//============================================================================== -template -void HierarchyTree::print(Node* root, int depth) { - for (int i = 0; i < depth; ++i) std::cout << " "; - std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " - << root->bv.min_[2] << "; " << root->bv.max_[0] << ", " - << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl; - if (root->isLeaf()) { - } else { - print(root->children[0], depth + 1); - print(root->children[1], depth + 1); - } -} - -//============================================================================== -template -void HierarchyTree::bottomup(const NodeVecIterator lbeg, - const NodeVecIterator lend) { - NodeVecIterator lcur_end = lend; - while (lbeg < lcur_end - 1) { - NodeVecIterator min_it1 = lbeg; - NodeVecIterator min_it2 = lbeg + 1; - FCL_REAL min_size = (std::numeric_limits::max)(); - for (NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1) { - for (NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2) { - FCL_REAL cur_size = ((*it1)->bv + (*it2)->bv).size(); - if (cur_size < min_size) { - min_size = cur_size; - min_it1 = it1; - min_it2 = it2; - } - } - } - - Node* n[2] = {*min_it1, *min_it2}; - Node* p = createNode(nullptr, n[0]->bv, n[1]->bv, nullptr); - p->children[0] = n[0]; - p->children[1] = n[1]; - n[0]->parent = p; - n[1]->parent = p; - *min_it1 = p; - Node* tmp = *min_it2; - lcur_end--; - *min_it2 = *lcur_end; - *lcur_end = tmp; - } -} - -//============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::topdown( - const NodeVecIterator lbeg, const NodeVecIterator lend) { - switch (topdown_level) { - case 0: - return topdown_0(lbeg, lend); - break; - case 1: - return topdown_1(lbeg, lend); - break; - default: - return topdown_0(lbeg, lend); - } -} - -//============================================================================== -template -size_t HierarchyTree::getMaxHeight(Node* node) const { - if (!node->isLeaf()) { - size_t height1 = getMaxHeight(node->children[0]); - size_t height2 = getMaxHeight(node->children[1]); - return std::max(height1, height2) + 1; - } else - return 0; -} - -//============================================================================== -template -void HierarchyTree::getMaxDepth(Node* node, size_t depth, - size_t& max_depth) const { - if (!node->isLeaf()) { - getMaxDepth(node->children[0], depth + 1, max_depth); - getMaxDepth(node->children[1], depth + 1, max_depth); - } else - max_depth = std::max(max_depth, depth); -} - -//============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::topdown_0( - const NodeVecIterator lbeg, const NodeVecIterator lend) { - long num_leaves = lend - lbeg; - if (num_leaves > 1) { - if (num_leaves > bu_threshold) { - BV vol = (*lbeg)->bv; - for (NodeVecIterator it = lbeg + 1; it < lend; ++it) vol += (*it)->bv; - - int best_axis = 0; - FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()}; - if (extent[1] > extent[0]) best_axis = 1; - if (extent[2] > extent[best_axis]) best_axis = 2; - - // compute median - NodeVecIterator lcenter = lbeg + num_leaves / 2; - std::nth_element(lbeg, lcenter, lend, - std::bind(&nodeBaseLess, std::placeholders::_1, - std::placeholders::_2, std::ref(best_axis))); - - Node* node = createNode(nullptr, vol, nullptr); - node->children[0] = topdown_0(lbeg, lcenter); - node->children[1] = topdown_0(lcenter, lend); - node->children[0]->parent = node; - node->children[1]->parent = node; - return node; - } else { - bottomup(lbeg, lend); - return *lbeg; - } - } - return *lbeg; -} - -//============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::topdown_1( - const NodeVecIterator lbeg, const NodeVecIterator lend) { - long num_leaves = lend - lbeg; - if (num_leaves > 1) { - if (num_leaves > bu_threshold) { - Vec3f split_p = (*lbeg)->bv.center(); - BV vol = (*lbeg)->bv; - NodeVecIterator it; - for (it = lbeg + 1; it < lend; ++it) { - split_p += (*it)->bv.center(); - vol += (*it)->bv; - } - split_p /= static_cast(num_leaves); - int best_axis = -1; - long bestmidp = num_leaves; - int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}}; - for (it = lbeg; it < lend; ++it) { - Vec3f x = (*it)->bv.center() - split_p; - for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0]; - } - - for (int i = 0; i < 3; ++i) { - if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { - long midp = std::abs(splitcount[i][0] - splitcount[i][1]); - if (midp < bestmidp) { - best_axis = i; - bestmidp = midp; - } - } - } - - if (best_axis < 0) best_axis = 0; - - FCL_REAL split_value = split_p[best_axis]; - NodeVecIterator lcenter = lbeg; - for (it = lbeg; it < lend; ++it) { - if ((*it)->bv.center()[best_axis] < split_value) { - Node* temp = *it; - *it = *lcenter; - *lcenter = temp; - ++lcenter; - } - } - - Node* node = createNode(nullptr, vol, nullptr); - node->children[0] = topdown_1(lbeg, lcenter); - node->children[1] = topdown_1(lcenter, lend); - node->children[0]->parent = node; - node->children[1]->parent = node; - return node; - } else { - bottomup(lbeg, lend); - return *lbeg; - } - } - return *lbeg; -} - -//============================================================================== -template -void HierarchyTree::init_0(std::vector& leaves) { - clear(); - root_node = topdown(leaves.begin(), leaves.end()); - n_leaves = leaves.size(); - max_lookahead_level = -1; - opath = 0; -} - -//============================================================================== -template -void HierarchyTree::init_1(std::vector& leaves) { - clear(); - - BV bound_bv; - if (leaves.size() > 0) bound_bv = leaves[0]->bv; - for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; - - morton_functor coder(bound_bv); - for (size_t i = 0; i < leaves.size(); ++i) - leaves[i]->code = coder(leaves[i]->bv.center()); - - std::sort(leaves.begin(), leaves.end(), SortByMorton()); - - root_node = mortonRecurse_0(leaves.begin(), leaves.end(), - (1 << (coder.bits() - 1)), coder.bits() - 1); - - refit(); - n_leaves = leaves.size(); - max_lookahead_level = -1; - opath = 0; -} - -//============================================================================== -template -void HierarchyTree::init_2(std::vector& leaves) { - clear(); - - BV bound_bv; - if (leaves.size() > 0) bound_bv = leaves[0]->bv; - for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; - - morton_functor coder(bound_bv); - for (size_t i = 0; i < leaves.size(); ++i) - leaves[i]->code = coder(leaves[i]->bv.center()); - - std::sort(leaves.begin(), leaves.end(), SortByMorton()); - - root_node = mortonRecurse_1(leaves.begin(), leaves.end(), - (1 << (coder.bits() - 1)), coder.bits() - 1); - - refit(); - n_leaves = leaves.size(); - max_lookahead_level = -1; - opath = 0; -} - -//============================================================================== -template -void HierarchyTree::init_3(std::vector& leaves) { - clear(); - - BV bound_bv; - if (leaves.size() > 0) bound_bv = leaves[0]->bv; - for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; - - morton_functor coder(bound_bv); - for (size_t i = 0; i < leaves.size(); ++i) - leaves[i]->code = coder(leaves[i]->bv.center()); - - std::sort(leaves.begin(), leaves.end(), SortByMorton()); - - root_node = mortonRecurse_2(leaves.begin(), leaves.end()); - - refit(); - n_leaves = leaves.size(); - max_lookahead_level = -1; - opath = 0; -} - -//============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::mortonRecurse_0( - const NodeVecIterator lbeg, const NodeVecIterator lend, - const uint32_t& split, int bits) { - long num_leaves = lend - lbeg; - if (num_leaves > 1) { - if (bits > 0) { - Node dummy; - dummy.code = split; - NodeVecIterator lcenter = - std::lower_bound(lbeg, lend, &dummy, SortByMorton()); - - if (lcenter == lbeg) { - uint32_t split2 = split | (1 << (bits - 1)); - return mortonRecurse_0(lbeg, lend, split2, bits - 1); - } else if (lcenter == lend) { - uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); - return mortonRecurse_0(lbeg, lend, split1, bits - 1); - } else { - uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); - uint32_t split2 = split | (1 << (bits - 1)); - - Node* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1); - Node* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1); - Node* node = createNode(nullptr, nullptr); - node->children[0] = child1; - node->children[1] = child2; - child1->parent = node; - child2->parent = node; - return node; - } - } else { - Node* node = topdown(lbeg, lend); - return node; - } - } else - return *lbeg; -} - -//============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::mortonRecurse_1( - const NodeVecIterator lbeg, const NodeVecIterator lend, - const uint32_t& split, int bits) { - long num_leaves = lend - lbeg; - if (num_leaves > 1) { - if (bits > 0) { - Node dummy; - dummy.code = split; - NodeVecIterator lcenter = - std::lower_bound(lbeg, lend, &dummy, SortByMorton()); - - if (lcenter == lbeg) { - uint32_t split2 = split | (1 << (bits - 1)); - return mortonRecurse_1(lbeg, lend, split2, bits - 1); - } else if (lcenter == lend) { - uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); - return mortonRecurse_1(lbeg, lend, split1, bits - 1); - } else { - uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); - uint32_t split2 = split | (1 << (bits - 1)); - - Node* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1); - Node* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1); - Node* node = createNode(nullptr, nullptr); - node->children[0] = child1; - node->children[1] = child2; - child1->parent = node; - child2->parent = node; - return node; - } - } else { - Node* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); - Node* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); - Node* node = createNode(nullptr, nullptr); - node->children[0] = child1; - node->children[1] = child2; - child1->parent = node; - child2->parent = node; - return node; - } - } else - return *lbeg; -} - -//============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::mortonRecurse_2( - const NodeVecIterator lbeg, const NodeVecIterator lend) { - long num_leaves = lend - lbeg; - if (num_leaves > 1) { - Node* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); - Node* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); - Node* node = createNode(nullptr, nullptr); - node->children[0] = child1; - node->children[1] = child2; - child1->parent = node; - child2->parent = node; - return node; - } else - return *lbeg; -} - -//============================================================================== -template -void HierarchyTree::update_(Node* leaf, const BV& bv) { - Node* root = removeLeaf(leaf); - if (root) { - if (max_lookahead_level >= 0) { - for (int i = 0; (i < max_lookahead_level) && root->parent; ++i) - root = root->parent; - } else - root = root_node; - } - - leaf->bv = bv; - insertLeaf(root, leaf); -} - -//============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::sort(Node* n, Node*& r) { - Node* p = n->parent; - if (p > n) { - size_t i = indexOf(n); - size_t j = 1 - i; - Node* s = p->children[j]; - Node* q = p->parent; - if (q) - q->children[indexOf(p)] = n; - else - r = n; - s->parent = n; - p->parent = n; - n->parent = q; - p->children[0] = n->children[0]; - p->children[1] = n->children[1]; - n->children[0]->parent = p; - n->children[1]->parent = p; - n->children[i] = p; - n->children[j] = s; - std::swap(p->bv, n->bv); - return p; - } - return n; -} - -//============================================================================== -template -void HierarchyTree::insertLeaf(Node* const sub_root, Node* const leaf) -// Attempts to insert `leaf` into a subtree rooted at `sub_root`. -// 1. If the whole tree is empty, then `leaf` simply becomes the tree. -// 2. Otherwise, a leaf node called `sibling` of the subtree rooted at -// `sub_root` is selected (the selection criteria is a black box for this -// algorithm), and the `sibling` leaf is replaced by an internal node with -// two children, `sibling` and `leaf`. The bounding volumes are updated as -// necessary. -{ - if (!root_node) { - // If the entire tree is empty, the node pointed by `leaf` variable will - // become the root of the tree. - root_node = leaf; - leaf->parent = nullptr; - return; - } - // Traverse the tree from the given `sub_root` down to an existing leaf - // node that we call `sibling`. The `sibling` node will eventually become - // the sibling of the given `leaf` node. - Node* sibling = sub_root; - while (!sibling->isLeaf()) { - sibling = sibling->children[select(*leaf, *(sibling->children[0]), - *(sibling->children[1]))]; - } - Node* prev = sibling->parent; - // Create a new `node` that later will become the new parent of both the - // `sibling` and the given `leaf`. - Node* node = createNode(prev, leaf->bv, sibling->bv, nullptr); - if (prev) - // If the parent `prev` of the `sibling` is an interior node, we will - // replace the `sibling` with the subtree {node {`sibling`, leaf}} like - // this: - // Before After - // - // ╱ ╱ - // prev prev - // ╱ ╲ ─> ╱ ╲ - // sibling ... node ... - // ╱ ╲ - // sibling leaf - { - prev->children[indexOf(sibling)] = node; - node->children[0] = sibling; - sibling->parent = node; - node->children[1] = leaf; - leaf->parent = node; - // Now that we've inserted `leaf` some of the existing bounding - // volumes might not fully enclose their children. Walk up the tree - // looking for parents that don't already enclose their children - // and create a new tight-fitting bounding volume for those. - do { - if (!prev->bv.contain(node->bv)) - prev->bv = prev->children[0]->bv + prev->children[1]->bv; - else - break; - node = prev; - } while (nullptr != (prev = node->parent)); - } else - // If the `sibling` has no parent, i.e., the tree is a singleton, - // we will replace it with the 3-node tree {node {`sibling`, `leaf`}} like - // this: - // - // node - // ╱ ╲ - // sibling leaf - { - node->children[0] = sibling; - sibling->parent = node; - node->children[1] = leaf; - leaf->parent = node; - root_node = node; - } - - // Note that the above algorithm always adds the new `leaf` node as the right - // child, i.e., children[1]. Calling removeLeaf(l) followed by calling - // this function insertLeaf(l) where l is a left child will result in - // switching l and its sibling even if no object's pose has changed. -} - -//============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::removeLeaf( - Node* const leaf) { - // Deletes `leaf` by replacing the subtree consisting of `leaf`, its sibling, - // and its parent with just its sibling. It then "tightens" the ancestor - // bounding volumes. Returns a pointer to the parent of the highest node whose - // BV changed due to the removal. - if (leaf == root_node) { - // If the `leaf` node is the only node in the tree, the tree becomes empty. - root_node = nullptr; - return nullptr; - } - Node* parent = leaf->parent; - Node* prev = parent->parent; - Node* sibling = parent->children[1 - indexOf(leaf)]; - if (prev) { - // If the parent node is not the root node, the sibling node will - // replace the parent node like this: - // - // Before After - // ... ... - // ╱ ╱ - // prev prev - // ╱ ╲ ╱ ╲ - // parent ... ─> sibling ... - // ╱ ╲ ╱╲ - // leaf sibling ... - // ╱╲ - // ... - // - // Step 1: replace the subtree {parent {leaf, sibling {...}}} with - // {sibling {...}}. - prev->children[indexOf(parent)] = sibling; - sibling->parent = prev; - deleteNode(parent); - // Step 2: tighten up the BVs of the ancestor nodes. - while (prev) { - BV new_bv = prev->children[0]->bv + prev->children[1]->bv; - if (!(new_bv == prev->bv)) { - prev->bv = new_bv; - prev = prev->parent; - } else - break; - } - - return prev ? prev : root_node; - } else { - // If the parent node is the root node, the sibling node will become the - // root of the whole tree like this: - // - // Before After - // - // parent - // ╱ ╲ - // leaf sibling ─> sibling - // ╱╲ ╱╲ - // ... ... - root_node = sibling; - sibling->parent = nullptr; - deleteNode(parent); - return root_node; - } -} - -//============================================================================== -template -void HierarchyTree::fetchLeaves(Node* root, std::vector& leaves, - int depth) { - if ((!root->isLeaf()) && depth) { - fetchLeaves(root->children[0], leaves, depth - 1); - fetchLeaves(root->children[1], leaves, depth - 1); - deleteNode(root); - } else { - leaves.push_back(root); - } -} - -//============================================================================== -template -size_t HierarchyTree::indexOf(Node* node) { - // node cannot be nullptr - return (node->parent->children[1] == node); -} - -//============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::createNode(Node* parent, - const BV& bv, - void* data) { - Node* node = createNode(parent, data); - node->bv = bv; - return node; -} - -//============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::createNode(Node* parent, - const BV& bv1, - const BV& bv2, - void* data) { - Node* node = createNode(parent, data); - node->bv = bv1 + bv2; - return node; -} - -//============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::createNode(Node* parent, - void* data) { - Node* node = nullptr; - if (free_node) { - node = free_node; - free_node = nullptr; - } else - node = new Node(); - node->parent = parent; - node->data = data; - node->children[1] = 0; - return node; -} - -//============================================================================== -template -void HierarchyTree::deleteNode(Node* node) { - if (free_node != node) { - delete free_node; - free_node = node; - } -} - -//============================================================================== -template -void HierarchyTree::recurseDeleteNode(Node* node) { - if (!node->isLeaf()) { - recurseDeleteNode(node->children[0]); - recurseDeleteNode(node->children[1]); - } - - if (node == root_node) root_node = nullptr; - deleteNode(node); -} - -//============================================================================== -template -void HierarchyTree::recurseRefit(Node* node) { - if (!node->isLeaf()) { - recurseRefit(node->children[0]); - recurseRefit(node->children[1]); - node->bv = node->children[0]->bv + node->children[1]->bv; - } else - return; -} - -//============================================================================== -template -bool nodeBaseLess(NodeBase* a, NodeBase* b, int d) { - if (a->bv.center()[d] < b->bv.center()[d]) return true; - return false; -} - -//============================================================================== -template -struct SelectImpl { - static std::size_t run(const NodeBase& /*query*/, - const NodeBase& /*node1*/, - const NodeBase& /*node2*/) { - return 0; - } - - static std::size_t run(const BV& /*query*/, const NodeBase& /*node1*/, - const NodeBase& /*node2*/) { - return 0; - } -}; - -//============================================================================== -template -size_t select(const NodeBase& query, const NodeBase& node1, - const NodeBase& node2) { - return SelectImpl::run(query, node1, node2); -} - -//============================================================================== -template -size_t select(const BV& query, const NodeBase& node1, - const NodeBase& node2) { - return SelectImpl::run(query, node1, node2); -} - -//============================================================================== -template -struct SelectImpl { - static std::size_t run(const NodeBase& node, - const NodeBase& node1, - const NodeBase& node2) { - const AABB& bv = node.bv; - const AABB& bv1 = node1.bv; - const AABB& bv2 = node2.bv; - Vec3f v = bv.min_ + bv.max_; - Vec3f v1 = v - (bv1.min_ + bv1.max_); - Vec3f v2 = v - (bv2.min_ + bv2.max_); - FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); - FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); - return (d1 < d2) ? 0 : 1; - } - - static std::size_t run(const AABB& query, const NodeBase& node1, - const NodeBase& node2) { - const AABB& bv = query; - const AABB& bv1 = node1.bv; - const AABB& bv2 = node2.bv; - Vec3f v = bv.min_ + bv.max_; - Vec3f v1 = v - (bv1.min_ + bv1.max_); - Vec3f v2 = v - (bv2.min_ + bv2.max_); - FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); - FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); - return (d1 < d2) ? 0 : 1; - } -}; - -} // namespace detail -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree.h index 54cddd079..ebacd648d 100644 --- a/include/hpp/fcl/broadphase/detail/hierarchy_tree.h +++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree.h @@ -1,294 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_HIERARCHY_TREE_H -#define HPP_FCL_HIERARCHY_TREE_H - -#include -#include -#include -#include -#include "hpp/fcl/warning.hh" -#include "hpp/fcl/BV/AABB.h" -#include "hpp/fcl/broadphase/detail/morton.h" -#include "hpp/fcl/broadphase/detail/node_base.h" - -namespace hpp { -namespace fcl { - -namespace detail { - -/// @brief Class for hierarchy tree structure -template -class HierarchyTree { - public: - typedef NodeBase Node; - - /// @brief Create hierarchy tree with suitable setting. - /// bu_threshold decides the height of tree node to start bottom-up - /// construction / optimization; topdown_level decides different methods to - /// construct tree in topdown manner. lower level method constructs tree with - /// better quality but is slower. - HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0); - - ~HierarchyTree(); - - /// @brief Initialize the tree by a set of leaves using algorithm with a given - /// level. - void init(std::vector& leaves, int level = 0); - - /// @brief Insest a node - Node* insert(const BV& bv, void* data); - - /// @brief Remove a leaf node - void remove(Node* leaf); - - /// @brief Clear the tree - void clear(); - - /// @brief Whether the tree is empty - bool empty() const; - - /// @brief Updates a `leaf` node. A use case is when the bounding volume - /// of an object changes. Ensure every parent node has its bounding volume - /// expand or shrink to fit the bounding volumes of its children. - /// @note Strangely the structure of the tree may change even if the - /// bounding volume of the `leaf` node does not change. It is just - /// another valid tree of the same set of objects. This is because - /// update() works by first removing `leaf` and then inserting `leaf` - /// back. The structural change could be as simple as switching the - /// order of two leaves if the sibling of the `leaf` is also a leaf. - /// Or it could be more complicated if the sibling is an internal - /// node. - void update(Node* leaf, int lookahead_level = -1); - - /// @brief update the tree when the bounding volume of a given leaf has - /// changed - bool update(Node* leaf, const BV& bv); - - /// @brief update one leaf's bounding volume, with prediction - bool update(Node* leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin); - - /// @brief update one leaf's bounding volume, with prediction - bool update(Node* leaf, const BV& bv, const Vec3f& vel); - - /// @brief get the max height of the tree - size_t getMaxHeight() const; - - /// @brief get the max depth of the tree - size_t getMaxDepth() const; - - /// @brief balance the tree from bottom - void balanceBottomup(); - - /// @brief balance the tree from top - void balanceTopdown(); - - /// @brief balance the tree in an incremental way - void balanceIncremental(int iterations); - - /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, - /// update the entire tree in a bottom-up manner - void refit(); - - /// @brief extract all the leaves of the tree - void extractLeaves(const Node* root, std::vector& leaves) const; - - /// @brief number of leaves in the tree - size_t size() const; - - /// @brief get the root of the tree - Node* getRoot() const; - - Node*& getRoot(); - - /// @brief print the tree in a recursive way - void print(Node* root, int depth); - - private: - typedef typename std::vector*>::iterator NodeVecIterator; - typedef - typename std::vector*>::const_iterator NodeVecConstIterator; - - struct SortByMorton { - bool operator()(const Node* a, const Node* b) const { - return a->code < b->code; - } - }; - - /// @brief construct a tree for a set of leaves from bottom -- very heavy way - void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend); - - /// @brief construct a tree for a set of leaves from top - Node* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend); - - /// @brief compute the maximum height of a subtree rooted from a given node - size_t getMaxHeight(Node* node) const; - - /// @brief compute the maximum depth of a subtree rooted from a given node - void getMaxDepth(Node* node, size_t depth, size_t& max_depth) const; - - /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a - /// topdown manner. During construction, first compute the best split axis as - /// the axis along with the longest AABB edge. Then compute the median of all - /// nodes' center projection onto the axis and using it as the split - /// threshold. - Node* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend); - - /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a - /// topdown manner. During construction, first compute the best split - /// thresholds for different axes as the average of all nodes' center. Then - /// choose the split axis as the axis whose threshold can divide the nodes - /// into two parts with almost similar size. This construction is more - /// expensive then topdown_0, but also can provide tree with better quality. - Node* topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend); - - /// @brief init tree from leaves in the topdown manner (topdown_0 or - /// topdown_1) - void init_0(std::vector& leaves); - - /// @brief init tree from leaves using morton code. It uses morton_0, i.e., - /// for nodes which is of depth more than the maximum bits of the morton code, - /// we use bottomup method to construct the subtree, which is slow but can - /// construct tree with high quality. - void init_1(std::vector& leaves); - - /// @brief init tree from leaves using morton code. It uses morton_0, i.e., - /// for nodes which is of depth more than the maximum bits of the morton code, - /// we split the leaves into two parts with the same size simply using the - /// node index. - void init_2(std::vector& leaves); - - /// @brief init tree from leaves using morton code. It uses morton_2, i.e., - /// for all nodes, we simply divide the leaves into parts with the same size - /// simply using the node index. - void init_3(std::vector& leaves); - - Node* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, - const uint32_t& split, int bits); - - Node* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, - const uint32_t& split, int bits); - - Node* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend); - - /// @brief update one leaf node's bounding volume - void update_(Node* leaf, const BV& bv); - - /// @brief sort node n and its parent according to their memory position - Node* sort(Node* n, Node*& r); - - /// @brief Insert a leaf node and also update its ancestors. Maintain the - /// tree as a full binary tree (every interior node has exactly two children). - /// Furthermore, adjust the BV of interior nodes so that each parent's BV - /// tightly fits its children's BVs. - /// @param sub_root The root of the subtree into which we will insert the - // leaf node. - void insertLeaf(Node* const sub_root, Node* const leaf); - - /// @brief Remove a leaf. Maintain the tree as a full binary tree (every - /// interior node has exactly two children). Furthermore, adjust the BV of - /// interior nodes so that each parent's BV tightly fits its children's BVs. - /// @note The leaf node itself is not deleted yet, but all the unnecessary - /// internal nodes are deleted. - /// @returns the root of the subtree containing the nodes whose BVs were - // adjusted. - Node* removeLeaf(Node* const leaf); - - /// @brief Delete all internal nodes and return all leaves nodes with given - /// depth from root - void fetchLeaves(Node* root, std::vector& leaves, int depth = -1); - - static size_t indexOf(Node* node); - - /// @brief create one node (leaf or internal) - Node* createNode(Node* parent, const BV& bv, void* data); - - Node* createNode(Node* parent, const BV& bv1, const BV& bv2, void* data); - - Node* createNode(Node* parent, void* data); - - void deleteNode(Node* node); - - void recurseDeleteNode(Node* node); - - void recurseRefit(Node* node); - - protected: - Node* root_node; - - size_t n_leaves; - - unsigned int opath; - - /// This is a one Node cache, the reason is that we need to remove a node and - /// then add it again frequently. - Node* free_node; - - int max_lookahead_level; - - public: - /// @brief decide which topdown algorithm to use - int topdown_level; - - /// @brief decide the depth to use expensive bottom-up algorithm - int bu_threshold; -}; - -/// @brief Compare two nodes accoording to the d-th dimension of node center -template -bool nodeBaseLess(NodeBase* a, NodeBase* b, int d); - -/// @brief select from node1 and node2 which is close to a given query. 0 for -/// node1 and 1 for node2 -template -size_t select(const NodeBase& query, const NodeBase& node1, - const NodeBase& node2); - -/// @brief select from node1 and node2 which is close to a given query bounding -/// volume. 0 for node1 and 1 for node2 -template -size_t select(const BV& query, const NodeBase& node1, - const NodeBase& node2); - -} // namespace detail -} // namespace fcl -} // namespace hpp - -#include "hpp/fcl/broadphase/detail/hierarchy_tree-inl.h" - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h index 8bb715fe9..a6923fbb7 100644 --- a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h +++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h @@ -1,980 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_HIERARCHY_TREE_ARRAY_INL_H -#define HPP_FCL_HIERARCHY_TREE_ARRAY_INL_H - -#include "hpp/fcl/broadphase/detail/hierarchy_tree_array.h" - -#include -#include - -namespace hpp { -namespace fcl { - -namespace detail { - -namespace implementation_array { - -//============================================================================== -template -HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) { - root_node = NULL_NODE; - n_nodes = 0; - n_nodes_alloc = 16; - nodes = new Node[n_nodes_alloc]; - for (size_t i = 0; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - n_leaves = 0; - freelist = 0; - opath = 0; - max_lookahead_level = -1; - bu_threshold = bu_threshold_; - topdown_level = topdown_level_; -} - -//============================================================================== -template -HierarchyTree::~HierarchyTree() { - delete[] nodes; -} - -//============================================================================== -template -void HierarchyTree::init(Node* leaves, int n_leaves_, int level) { - switch (level) { - case 0: - init_0(leaves, n_leaves_); - break; - case 1: - init_1(leaves, n_leaves_); - break; - case 2: - init_2(leaves, n_leaves_); - break; - case 3: - init_3(leaves, n_leaves_); - break; - default: - init_0(leaves, n_leaves_); - } -} - -//============================================================================== -template -void HierarchyTree::init_0(Node* leaves, int n_leaves_) { - clear(); - - n_leaves = (size_t)n_leaves_; - root_node = NULL_NODE; - nodes = new Node[n_leaves * 2]; - std::copy(leaves, leaves + n_leaves, nodes); - freelist = n_leaves; - n_nodes = n_leaves; - n_nodes_alloc = 2 * n_leaves; - for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - - size_t* ids = new size_t[n_leaves]; - for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; - - root_node = topdown(ids, ids + n_leaves); - delete[] ids; - - opath = 0; - max_lookahead_level = -1; -} - -//============================================================================== -template -void HierarchyTree::init_1(Node* leaves, int n_leaves_) { - clear(); - - n_leaves = (size_t)n_leaves_; - root_node = NULL_NODE; - nodes = new Node[n_leaves * 2]; - std::copy(leaves, leaves + n_leaves, nodes); - freelist = n_leaves; - n_nodes = n_leaves; - n_nodes_alloc = 2 * n_leaves; - for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - - BV bound_bv; - if (n_leaves > 0) bound_bv = nodes[0].bv; - for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; - - morton_functor coder(bound_bv); - for (size_t i = 0; i < n_leaves; ++i) - nodes[i].code = coder(nodes[i].bv.center()); - - size_t* ids = new size_t[n_leaves]; - for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; - - const SortByMorton comp{nodes}; - std::sort(ids, ids + n_leaves, comp); - root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << (coder.bits() - 1)), - coder.bits() - 1); - delete[] ids; - - refit(); - - opath = 0; - max_lookahead_level = -1; -} - -//============================================================================== -template -void HierarchyTree::init_2(Node* leaves, int n_leaves_) { - clear(); - - n_leaves = (size_t)n_leaves_; - root_node = NULL_NODE; - nodes = new Node[n_leaves * 2]; - std::copy(leaves, leaves + n_leaves, nodes); - freelist = n_leaves; - n_nodes = n_leaves; - n_nodes_alloc = 2 * n_leaves; - for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - - BV bound_bv; - if (n_leaves > 0) bound_bv = nodes[0].bv; - for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; - - morton_functor coder(bound_bv); - for (size_t i = 0; i < n_leaves; ++i) - nodes[i].code = coder(nodes[i].bv.center()); - - size_t* ids = new size_t[n_leaves]; - for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; - - const SortByMorton comp{nodes}; - std::sort(ids, ids + n_leaves, comp); - root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << (coder.bits() - 1)), - coder.bits() - 1); - delete[] ids; - - refit(); - - opath = 0; - max_lookahead_level = -1; -} - -//============================================================================== -template -void HierarchyTree::init_3(Node* leaves, int n_leaves_) { - clear(); - - n_leaves = (size_t)n_leaves_; - root_node = NULL_NODE; - nodes = new Node[n_leaves * 2]; - std::copy(leaves, leaves + n_leaves, nodes); - freelist = n_leaves; - n_nodes = n_leaves; - n_nodes_alloc = 2 * n_leaves; - for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - - BV bound_bv; - if (n_leaves > 0) bound_bv = nodes[0].bv; - for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; - - morton_functor coder(bound_bv); - for (size_t i = 0; i < n_leaves; ++i) - nodes[i].code = coder(nodes[i].bv.center()); - - size_t* ids = new size_t[n_leaves]; - for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; - - const SortByMorton comp{nodes}; - std::sort(ids, ids + n_leaves, comp); - root_node = mortonRecurse_2(ids, ids + n_leaves); - delete[] ids; - - refit(); - - opath = 0; - max_lookahead_level = -1; -} - -//============================================================================== -template -size_t HierarchyTree::insert(const BV& bv, void* data) { - size_t node = createNode(NULL_NODE, bv, data); - insertLeaf(root_node, node); - ++n_leaves; - return node; -} - -//============================================================================== -template -void HierarchyTree::remove(size_t leaf) { - removeLeaf(leaf); - deleteNode(leaf); - --n_leaves; -} - -//============================================================================== -template -void HierarchyTree::clear() { - delete[] nodes; - root_node = NULL_NODE; - n_nodes = 0; - n_nodes_alloc = 16; - nodes = new Node[n_nodes_alloc]; - for (size_t i = 0; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - n_leaves = 0; - freelist = 0; - opath = 0; - max_lookahead_level = -1; -} - -//============================================================================== -template -bool HierarchyTree::empty() const { - return (n_nodes == 0); -} - -//============================================================================== -template -void HierarchyTree::update(size_t leaf, int lookahead_level) { - size_t root = removeLeaf(leaf); - if (root != NULL_NODE) { - if (lookahead_level > 0) { - for (int i = 0; - (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) - root = nodes[root].parent; - } else - root = root_node; - } - insertLeaf(root, leaf); -} - -//============================================================================== -template -bool HierarchyTree::update(size_t leaf, const BV& bv) { - if (nodes[leaf].bv.contain(bv)) return false; - update_(leaf, bv); - return true; -} - -//============================================================================== -template -bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3f& vel, - FCL_REAL margin) { - HPP_FCL_UNUSED_VARIABLE(bv); - HPP_FCL_UNUSED_VARIABLE(vel); - HPP_FCL_UNUSED_VARIABLE(margin); - - if (nodes[leaf].bv.contain(bv)) return false; - update_(leaf, bv); - return true; -} - -//============================================================================== -template -bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3f& vel) { - HPP_FCL_UNUSED_VARIABLE(vel); - - if (nodes[leaf].bv.contain(bv)) return false; - update_(leaf, bv); - return true; -} - -//============================================================================== -template -size_t HierarchyTree::getMaxHeight() const { - if (root_node == NULL_NODE) return 0; - - return getMaxHeight(root_node); -} - -//============================================================================== -template -size_t HierarchyTree::getMaxDepth() const { - if (root_node == NULL_NODE) return 0; - - size_t max_depth; - getMaxDepth(root_node, 0, max_depth); - return max_depth; -} - -//============================================================================== -template -void HierarchyTree::balanceBottomup() { - if (root_node != NULL_NODE) { - Node* leaves = new Node[n_leaves]; - Node* leaves_ = leaves; - extractLeaves(root_node, leaves_); - root_node = NULL_NODE; - std::copy(leaves, leaves + n_leaves, nodes); - freelist = n_leaves; - n_nodes = n_leaves; - for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - - size_t* ids = new size_t[n_leaves]; - for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; - - bottomup(ids, ids + n_leaves); - root_node = *ids; - - delete[] ids; - } -} - -//============================================================================== -template -void HierarchyTree::balanceTopdown() { - if (root_node != NULL_NODE) { - Node* leaves = new Node[n_leaves]; - Node* leaves_ = leaves; - extractLeaves(root_node, leaves_); - root_node = NULL_NODE; - std::copy(leaves, leaves + n_leaves, nodes); - freelist = n_leaves; - n_nodes = n_leaves; - for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - - size_t* ids = new size_t[n_leaves]; - for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; - - root_node = topdown(ids, ids + n_leaves); - delete[] ids; - } -} - -//============================================================================== -template -void HierarchyTree::balanceIncremental(int iterations) { - if (iterations < 0) iterations = (int)n_leaves; - if ((root_node != NULL_NODE) && (iterations > 0)) { - for (int i = 0; i < iterations; ++i) { - size_t node = root_node; - unsigned int bit = 0; - while (!nodes[node].isLeaf()) { - node = nodes[node].children[(opath >> bit) & 1]; - bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1); - } - update(node); - ++opath; - } - } -} - -//============================================================================== -template -void HierarchyTree::refit() { - if (root_node != NULL_NODE) recurseRefit(root_node); -} - -//============================================================================== -template -void HierarchyTree::extractLeaves(size_t root, Node*& leaves) const { - if (!nodes[root].isLeaf()) { - extractLeaves(nodes[root].children[0], leaves); - extractLeaves(nodes[root].children[1], leaves); - } else { - *leaves = nodes[root]; - leaves++; - } -} - -//============================================================================== -template -size_t HierarchyTree::size() const { - return n_leaves; -} - -//============================================================================== -template -size_t HierarchyTree::getRoot() const { - return root_node; -} - -//============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::getNodes() const { - return nodes; -} - -//============================================================================== -template -void HierarchyTree::print(size_t root, int depth) { - for (int i = 0; i < depth; ++i) std::cout << " "; - Node* n = nodes + root; - std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", " - << n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1] - << ", " << n->bv.max_[2] << ")" << std::endl; - if (n->isLeaf()) { - } else { - print(n->children[0], depth + 1); - print(n->children[1], depth + 1); - } -} - -//============================================================================== -template -size_t HierarchyTree::getMaxHeight(size_t node) const { - if (!nodes[node].isLeaf()) { - size_t height1 = getMaxHeight(nodes[node].children[0]); - size_t height2 = getMaxHeight(nodes[node].children[1]); - return std::max(height1, height2) + 1; - } else - return 0; -} - -//============================================================================== -template -void HierarchyTree::getMaxDepth(size_t node, size_t depth, - size_t& max_depth) const { - if (!nodes[node].isLeaf()) { - getMaxDepth(nodes[node].children[0], depth + 1, max_depth); - getmaxDepth(nodes[node].children[1], depth + 1, max_depth); - } else - max_depth = std::max(max_depth, depth); -} - -//============================================================================== -template -void HierarchyTree::bottomup(size_t* lbeg, size_t* lend) { - size_t* lcur_end = lend; - while (lbeg < lcur_end - 1) { - size_t *min_it1 = nullptr, *min_it2 = nullptr; - FCL_REAL min_size = (std::numeric_limits::max)(); - for (size_t* it1 = lbeg; it1 < lcur_end; ++it1) { - for (size_t* it2 = it1 + 1; it2 < lcur_end; ++it2) { - FCL_REAL cur_size = (nodes[*it1].bv + nodes[*it2].bv).size(); - if (cur_size < min_size) { - min_size = cur_size; - min_it1 = it1; - min_it2 = it2; - } - } - } - - size_t p = - createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, nullptr); - nodes[p].children[0] = *min_it1; - nodes[p].children[1] = *min_it2; - nodes[*min_it1].parent = p; - nodes[*min_it2].parent = p; - *min_it1 = p; - size_t tmp = *min_it2; - lcur_end--; - *min_it2 = *lcur_end; - *lcur_end = tmp; - } -} - -//============================================================================== -template -size_t HierarchyTree::topdown(size_t* lbeg, size_t* lend) { - switch (topdown_level) { - case 0: - return topdown_0(lbeg, lend); - break; - case 1: - return topdown_1(lbeg, lend); - break; - default: - return topdown_0(lbeg, lend); - } -} - -//============================================================================== -template -size_t HierarchyTree::topdown_0(size_t* lbeg, size_t* lend) { - long num_leaves = lend - lbeg; - if (num_leaves > 1) { - if (num_leaves > bu_threshold) { - BV vol = nodes[*lbeg].bv; - for (size_t* i = lbeg + 1; i < lend; ++i) vol += nodes[*i].bv; - - size_t best_axis = 0; - FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()}; - if (extent[1] > extent[0]) best_axis = 1; - if (extent[2] > extent[best_axis]) best_axis = 2; - - nodeBaseLess comp(nodes, best_axis); - size_t* lcenter = lbeg + num_leaves / 2; - std::nth_element(lbeg, lcenter, lend, comp); - - size_t node = createNode(NULL_NODE, vol, nullptr); - nodes[node].children[0] = topdown_0(lbeg, lcenter); - nodes[node].children[1] = topdown_0(lcenter, lend); - nodes[nodes[node].children[0]].parent = node; - nodes[nodes[node].children[1]].parent = node; - return node; - } else { - bottomup(lbeg, lend); - return *lbeg; - } - } - return *lbeg; -} - -//============================================================================== -template -size_t HierarchyTree::topdown_1(size_t* lbeg, size_t* lend) { - long num_leaves = lend - lbeg; - if (num_leaves > 1) { - if (num_leaves > bu_threshold) { - Vec3f split_p = nodes[*lbeg].bv.center(); - BV vol = nodes[*lbeg].bv; - for (size_t* i = lbeg + 1; i < lend; ++i) { - split_p += nodes[*i].bv.center(); - vol += nodes[*i].bv; - } - split_p /= static_cast(num_leaves); - int best_axis = -1; - int bestmidp = (int)num_leaves; - int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}}; - for (size_t* i = lbeg; i < lend; ++i) { - Vec3f x = nodes[*i].bv.center() - split_p; - for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0]; - } - - for (size_t i = 0; i < 3; ++i) { - if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { - int midp = std::abs(splitcount[i][0] - splitcount[i][1]); - if (midp < bestmidp) { - best_axis = (int)i; - bestmidp = midp; - } - } - } - - if (best_axis < 0) best_axis = 0; - - FCL_REAL split_value = split_p[best_axis]; - size_t* lcenter = lbeg; - for (size_t* i = lbeg; i < lend; ++i) { - if (nodes[*i].bv.center()[best_axis] < split_value) { - size_t temp = *i; - *i = *lcenter; - *lcenter = temp; - ++lcenter; - } - } - - size_t node = createNode(NULL_NODE, vol, nullptr); - nodes[node].children[0] = topdown_1(lbeg, lcenter); - nodes[node].children[1] = topdown_1(lcenter, lend); - nodes[nodes[node].children[0]].parent = node; - nodes[nodes[node].children[1]].parent = node; - return node; - } else { - bottomup(lbeg, lend); - return *lbeg; - } - } - return *lbeg; -} - -//============================================================================== -template -size_t HierarchyTree::mortonRecurse_0(size_t* lbeg, size_t* lend, - const uint32_t& split, int bits) { - long num_leaves = lend - lbeg; - if (num_leaves > 1) { - if (bits > 0) { - const SortByMorton comp{nodes, split}; - size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp); - - if (lcenter == lbeg) { - uint32_t split2 = split | (1 << (bits - 1)); - return mortonRecurse_0(lbeg, lend, split2, bits - 1); - } else if (lcenter == lend) { - uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); - return mortonRecurse_0(lbeg, lend, split1, bits - 1); - } else { - uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); - uint32_t split2 = split | (1 << (bits - 1)); - - size_t child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1); - size_t child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1); - size_t node = createNode(NULL_NODE, nullptr); - nodes[node].children[0] = child1; - nodes[node].children[1] = child2; - nodes[child1].parent = node; - nodes[child2].parent = node; - return node; - } - } else { - size_t node = topdown(lbeg, lend); - return node; - } - } else - return *lbeg; -} - -//============================================================================== -template -size_t HierarchyTree::mortonRecurse_1(size_t* lbeg, size_t* lend, - const uint32_t& split, int bits) { - long num_leaves = lend - lbeg; - if (num_leaves > 1) { - if (bits > 0) { - const SortByMorton comp{nodes, split}; - size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp); - - if (lcenter == lbeg) { - uint32_t split2 = split | (1 << (bits - 1)); - return mortonRecurse_1(lbeg, lend, split2, bits - 1); - } else if (lcenter == lend) { - uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); - return mortonRecurse_1(lbeg, lend, split1, bits - 1); - } else { - uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); - uint32_t split2 = split | (1 << (bits - 1)); - - size_t child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1); - size_t child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1); - size_t node = createNode(NULL_NODE, nullptr); - nodes[node].children[0] = child1; - nodes[node].children[1] = child2; - nodes[child1].parent = node; - nodes[child2].parent = node; - return node; - } - } else { - size_t child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); - size_t child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); - size_t node = createNode(NULL_NODE, nullptr); - nodes[node].children[0] = child1; - nodes[node].children[1] = child2; - nodes[child1].parent = node; - nodes[child2].parent = node; - return node; - } - } else - return *lbeg; -} - -//============================================================================== -template -size_t HierarchyTree::mortonRecurse_2(size_t* lbeg, size_t* lend) { - long num_leaves = lend - lbeg; - if (num_leaves > 1) { - size_t child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); - size_t child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); - size_t node = createNode(NULL_NODE, nullptr); - nodes[node].children[0] = child1; - nodes[node].children[1] = child2; - nodes[child1].parent = node; - nodes[child2].parent = node; - return node; - } else - return *lbeg; -} - -//============================================================================== -template -void HierarchyTree::insertLeaf(size_t root, size_t leaf) { - if (root_node == NULL_NODE) { - root_node = leaf; - nodes[leaf].parent = NULL_NODE; - } else { - if (!nodes[root].isLeaf()) { - do { - root = nodes[root].children[select(leaf, nodes[root].children[0], - nodes[root].children[1], nodes)]; - } while (!nodes[root].isLeaf()); - } - - size_t prev = nodes[root].parent; - size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, nullptr); - if (prev != NULL_NODE) { - nodes[prev].children[indexOf(root)] = node; - nodes[node].children[0] = root; - nodes[root].parent = node; - nodes[node].children[1] = leaf; - nodes[leaf].parent = node; - do { - if (!nodes[prev].bv.contain(nodes[node].bv)) - nodes[prev].bv = nodes[nodes[prev].children[0]].bv + - nodes[nodes[prev].children[1]].bv; - else - break; - node = prev; - } while (NULL_NODE != (prev = nodes[node].parent)); - } else { - nodes[node].children[0] = root; - nodes[root].parent = node; - nodes[node].children[1] = leaf; - nodes[leaf].parent = node; - root_node = node; - } - } -} - -//============================================================================== -template -size_t HierarchyTree::removeLeaf(size_t leaf) { - if (leaf == root_node) { - root_node = NULL_NODE; - return NULL_NODE; - } else { - size_t parent = nodes[leaf].parent; - size_t prev = nodes[parent].parent; - size_t sibling = nodes[parent].children[1 - indexOf(leaf)]; - - if (prev != NULL_NODE) { - nodes[prev].children[indexOf(parent)] = sibling; - nodes[sibling].parent = prev; - deleteNode(parent); - while (prev != NULL_NODE) { - BV new_bv = nodes[nodes[prev].children[0]].bv + - nodes[nodes[prev].children[1]].bv; - if (!(new_bv == nodes[prev].bv)) { - nodes[prev].bv = new_bv; - prev = nodes[prev].parent; - } else - break; - } - - return (prev != NULL_NODE) ? prev : root_node; - } else { - root_node = sibling; - nodes[sibling].parent = NULL_NODE; - deleteNode(parent); - return root_node; - } - } -} - -//============================================================================== -template -void HierarchyTree::update_(size_t leaf, const BV& bv) { - size_t root = removeLeaf(leaf); - if (root != NULL_NODE) { - if (max_lookahead_level >= 0) { - for (int i = 0; - (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) - root = nodes[root].parent; - } - - nodes[leaf].bv = bv; - insertLeaf(root, leaf); - } -} - -//============================================================================== -template -size_t HierarchyTree::indexOf(size_t node) { - return (nodes[nodes[node].parent].children[1] == node); -} - -//============================================================================== -template -size_t HierarchyTree::allocateNode() { - if (freelist == NULL_NODE) { - Node* old_nodes = nodes; - n_nodes_alloc *= 2; - nodes = new Node[n_nodes_alloc]; - std::copy(old_nodes, old_nodes + n_nodes, nodes); - delete[] old_nodes; - - for (size_t i = n_nodes; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - freelist = n_nodes; - } - - size_t node_id = freelist; - freelist = nodes[node_id].next; - nodes[node_id].parent = NULL_NODE; - nodes[node_id].children[0] = NULL_NODE; - nodes[node_id].children[1] = NULL_NODE; - ++n_nodes; - return node_id; -} - -//============================================================================== -template -size_t HierarchyTree::createNode(size_t parent, const BV& bv1, - const BV& bv2, void* data) { - size_t node = allocateNode(); - nodes[node].parent = parent; - nodes[node].data = data; - nodes[node].bv = bv1 + bv2; - return node; -} - -//============================================================================== -template -size_t HierarchyTree::createNode(size_t parent, const BV& bv, void* data) { - size_t node = allocateNode(); - nodes[node].parent = parent; - nodes[node].data = data; - nodes[node].bv = bv; - return node; -} - -//============================================================================== -template -size_t HierarchyTree::createNode(size_t parent, void* data) { - size_t node = allocateNode(); - nodes[node].parent = parent; - nodes[node].data = data; - return node; -} - -//============================================================================== -template -void HierarchyTree::deleteNode(size_t node) { - nodes[node].next = freelist; - freelist = node; - --n_nodes; -} - -//============================================================================== -template -void HierarchyTree::recurseRefit(size_t node) { - if (!nodes[node].isLeaf()) { - recurseRefit(nodes[node].children[0]); - recurseRefit(nodes[node].children[1]); - nodes[node].bv = - nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv; - } else - return; -} - -//============================================================================== -template -void HierarchyTree::fetchLeaves(size_t root, Node*& leaves, int depth) { - if ((!nodes[root].isLeaf()) && depth) { - fetchLeaves(nodes[root].children[0], leaves, depth - 1); - fetchLeaves(nodes[root].children[1], leaves, depth - 1); - deleteNode(root); - } else { - *leaves = nodes[root]; - leaves++; - } -} - -//============================================================================== -template -nodeBaseLess::nodeBaseLess(const NodeBase* nodes_, size_t d_) - : nodes(nodes_), d(d_) { - // Do nothing -} - -//============================================================================== -template -bool nodeBaseLess::operator()(size_t i, size_t j) const { - if (nodes[i].bv.center()[(int)d] < nodes[j].bv.center()[(int)d]) return true; - - return false; -} - -//============================================================================== -template -struct SelectImpl { - static bool run(size_t query, size_t node1, size_t node2, - NodeBase* nodes) { - HPP_FCL_UNUSED_VARIABLE(query); - HPP_FCL_UNUSED_VARIABLE(node1); - HPP_FCL_UNUSED_VARIABLE(node2); - HPP_FCL_UNUSED_VARIABLE(nodes); - - return 0; - } - - static bool run(const BV& query, size_t node1, size_t node2, - NodeBase* nodes) { - HPP_FCL_UNUSED_VARIABLE(query); - HPP_FCL_UNUSED_VARIABLE(node1); - HPP_FCL_UNUSED_VARIABLE(node2); - HPP_FCL_UNUSED_VARIABLE(nodes); - - return 0; - } -}; - -//============================================================================== -template -size_t select(size_t query, size_t node1, size_t node2, NodeBase* nodes) { - return SelectImpl::run(query, node1, node2, nodes); -} - -//============================================================================== -template -size_t select(const BV& query, size_t node1, size_t node2, - NodeBase* nodes) { - return SelectImpl::run(query, node1, node2, nodes); -} - -//============================================================================== -template -struct SelectImpl { - static bool run(size_t query, size_t node1, size_t node2, - NodeBase* nodes) { - const AABB& bv = nodes[query].bv; - const AABB& bv1 = nodes[node1].bv; - const AABB& bv2 = nodes[node2].bv; - Vec3f v = bv.min_ + bv.max_; - Vec3f v1 = v - (bv1.min_ + bv1.max_); - Vec3f v2 = v - (bv2.min_ + bv2.max_); - FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); - FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); - return (d1 < d2) ? 0 : 1; - } - - static bool run(const AABB& query, size_t node1, size_t node2, - NodeBase* nodes) { - const AABB& bv = query; - const AABB& bv1 = nodes[node1].bv; - const AABB& bv2 = nodes[node2].bv; - Vec3f v = bv.min_ + bv.max_; - Vec3f v1 = v - (bv1.min_ + bv1.max_); - Vec3f v2 = v - (bv2.min_ + bv2.max_); - FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); - FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); - return (d1 < d2) ? 0 : 1; - } -}; - -} // namespace implementation_array -} // namespace detail -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h index 8709a848b..e0c9a90a1 100644 --- a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h +++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h @@ -1,302 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_HIERARCHY_TREE_ARRAY_H -#define HPP_FCL_HIERARCHY_TREE_ARRAY_H - -#include -#include -#include - -#include "hpp/fcl/fwd.hh" -#include "hpp/fcl/BV/AABB.h" -#include "hpp/fcl/broadphase/detail/morton.h" -#include "hpp/fcl/broadphase/detail/node_base_array.h" - -namespace hpp { -namespace fcl { - -namespace detail { - -namespace implementation_array { - -/// @brief Class for hierarchy tree structure -template -class HierarchyTree { - public: - typedef NodeBase Node; - - private: - struct SortByMorton { - SortByMorton(Node* nodes_in) : nodes(nodes_in) {} - SortByMorton(Node* nodes_in, uint32_t split_in) - : nodes(nodes_in), split(split_in) {} - bool operator()(size_t a, size_t b) const { - if ((a != NULL_NODE) && (b != NULL_NODE)) - return nodes[a].code < nodes[b].code; - else if (a == NULL_NODE) - return split < nodes[b].code; - else if (b == NULL_NODE) - return nodes[a].code < split; - - return false; - } - - Node* nodes{}; - uint32_t split{}; - }; - - public: - /// @brief Create hierarchy tree with suitable setting. - /// bu_threshold decides the height of tree node to start bottom-up - /// construction / optimization; topdown_level decides different methods to - /// construct tree in topdown manner. lower level method constructs tree with - /// better quality but is slower. - HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0); - - ~HierarchyTree(); - - /// @brief Initialize the tree by a set of leaves using algorithm with a given - /// level. - void init(Node* leaves, int n_leaves_, int level = 0); - - /// @brief Initialize the tree by a set of leaves using algorithm with a given - /// level. - size_t insert(const BV& bv, void* data); - - /// @brief Remove a leaf node - void remove(size_t leaf); - - /// @brief Clear the tree - void clear(); - - /// @brief Whether the tree is empty - bool empty() const; - - /// @brief update one leaf node - void update(size_t leaf, int lookahead_level = -1); - - /// @brief update the tree when the bounding volume of a given leaf has - /// changed - bool update(size_t leaf, const BV& bv); - - /// @brief update one leaf's bounding volume, with prediction - bool update(size_t leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin); - - /// @brief update one leaf's bounding volume, with prediction - bool update(size_t leaf, const BV& bv, const Vec3f& vel); - - /// @brief get the max height of the tree - size_t getMaxHeight() const; - - /// @brief get the max depth of the tree - size_t getMaxDepth() const; - - /// @brief balance the tree from bottom - void balanceBottomup(); - - /// @brief balance the tree from top - void balanceTopdown(); - - /// @brief balance the tree in an incremental way - void balanceIncremental(int iterations); - - /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, - /// update the entire tree in a bottom-up manner - void refit(); - - /// @brief extract all the leaves of the tree - void extractLeaves(size_t root, Node*& leaves) const; - - /// @brief number of leaves in the tree - size_t size() const; - - /// @brief get the root of the tree - size_t getRoot() const; - - /// @brief get the pointer to the nodes array - Node* getNodes() const; - - /// @brief print the tree in a recursive way - void print(size_t root, int depth); - - private: - /// @brief construct a tree for a set of leaves from bottom -- very heavy way - void bottomup(size_t* lbeg, size_t* lend); - - /// @brief construct a tree for a set of leaves from top - size_t topdown(size_t* lbeg, size_t* lend); - - /// @brief compute the maximum height of a subtree rooted from a given node - size_t getMaxHeight(size_t node) const; - - /// @brief compute the maximum depth of a subtree rooted from a given node - void getMaxDepth(size_t node, size_t depth, size_t& max_depth) const; - - /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a - /// topdown manner. During construction, first compute the best split axis as - /// the axis along with the longest AABB edge. Then compute the median of all - /// nodes' center projection onto the axis and using it as the split - /// threshold. - size_t topdown_0(size_t* lbeg, size_t* lend); - - /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a - /// topdown manner. During construction, first compute the best split - /// thresholds for different axes as the average of all nodes' center. Then - /// choose the split axis as the axis whose threshold can divide the nodes - /// into two parts with almost similar size. This construction is more - /// expensive then topdown_0, but also can provide tree with better quality. - size_t topdown_1(size_t* lbeg, size_t* lend); - - /// @brief init tree from leaves in the topdown manner (topdown_0 or - /// topdown_1) - void init_0(Node* leaves, int n_leaves_); - - /// @brief init tree from leaves using morton code. It uses morton_0, i.e., - /// for nodes which is of depth more than the maximum bits of the morton code, - /// we use bottomup method to construct the subtree, which is slow but can - /// construct tree with high quality. - void init_1(Node* leaves, int n_leaves_); - - /// @brief init tree from leaves using morton code. It uses morton_0, i.e., - /// for nodes which is of depth more than the maximum bits of the morton code, - /// we split the leaves into two parts with the same size simply using the - /// node index. - void init_2(Node* leaves, int n_leaves_); - - /// @brief init tree from leaves using morton code. It uses morton_2, i.e., - /// for all nodes, we simply divide the leaves into parts with the same size - /// simply using the node index. - void init_3(Node* leaves, int n_leaves_); - - size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const uint32_t& split, - int bits); - - size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const uint32_t& split, - int bits); - - size_t mortonRecurse_2(size_t* lbeg, size_t* lend); - - /// @brief update one leaf node's bounding volume - void update_(size_t leaf, const BV& bv); - - /// @brief Insert a leaf node and also update its ancestors - void insertLeaf(size_t root, size_t leaf); - - /// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the - /// unnecessary internal nodes are deleted. return the node with the smallest - /// depth and is influenced by the remove operation - size_t removeLeaf(size_t leaf); - - /// @brief Delete all internal nodes and return all leaves nodes with given - /// depth from root - void fetchLeaves(size_t root, Node*& leaves, int depth = -1); - - size_t indexOf(size_t node); - - size_t allocateNode(); - - /// @brief create one node (leaf or internal) - size_t createNode(size_t parent, const BV& bv1, const BV& bv2, void* data); - - size_t createNode(size_t parent, const BV& bv, void* data); - - size_t createNode(size_t parent, void* data); - - void deleteNode(size_t node); - - void recurseRefit(size_t node); - - protected: - size_t root_node; - Node* nodes; - size_t n_nodes; - size_t n_nodes_alloc; - - size_t n_leaves; - size_t freelist; - unsigned int opath; - - int max_lookahead_level; - - public: - /// @brief decide which topdown algorithm to use - int topdown_level; - - /// @brief decide the depth to use expensive bottom-up algorithm - int bu_threshold; - - public: - static const size_t NULL_NODE = std::numeric_limits::max(); -}; - -template -const size_t HierarchyTree::NULL_NODE; - -/// @brief Functor comparing two nodes -template -struct nodeBaseLess { - nodeBaseLess(const NodeBase* nodes_, size_t d_); - - bool operator()(size_t i, size_t j) const; - - private: - /// @brief the nodes array - const NodeBase* nodes; - - /// @brief the dimension to compare - size_t d; -}; - -/// @brief select the node from node1 and node2 which is close to the query-th -/// node in the nodes. 0 for node1 and 1 for node2. -template -size_t select(size_t query, size_t node1, size_t node2, NodeBase* nodes); - -/// @brief select the node from node1 and node2 which is close to the query -/// AABB. 0 for node1 and 1 for node2. -template -size_t select(const BV& query, size_t node1, size_t node2, NodeBase* nodes); - -} // namespace implementation_array -} // namespace detail -} // namespace fcl -} // namespace hpp - -#include "hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h" - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/interval_tree.h b/include/hpp/fcl/broadphase/detail/interval_tree.h index bbcb1fec8..5480fc00f 100644 --- a/include/hpp/fcl/broadphase/detail/interval_tree.h +++ b/include/hpp/fcl/broadphase/detail/interval_tree.h @@ -1,129 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_INTERVAL_TREE_H -#define HPP_FCL_INTERVAL_TREE_H - -#include -#include -#include - -#include "hpp/fcl/broadphase/detail/interval_tree_node.h" - -namespace hpp { -namespace fcl { -namespace detail { - -/// @brief Class describes the information needed when we take the -/// right branch in searching for intervals but possibly come back -/// and check the left branch as well. -struct HPP_FCL_DLLAPI it_recursion_node { - public: - IntervalTreeNode* start_node; - - unsigned int parent_index; - - bool try_right_branch; -}; - -/// @brief Interval tree -class HPP_FCL_DLLAPI IntervalTree { - public: - IntervalTree(); - - ~IntervalTree(); - - /// @brief Print the whole interval tree - void print() const; - - /// @brief Delete one node of the interval tree - SimpleInterval* deleteNode(IntervalTreeNode* node); - - /// @brief delete node stored a given interval - void deleteNode(SimpleInterval* ivl); - - /// @brief Insert one node of the interval tree - IntervalTreeNode* insert(SimpleInterval* new_interval); - - /// @brief get the predecessor of a given node - IntervalTreeNode* getPredecessor(IntervalTreeNode* node) const; - - /// @brief Get the successor of a given node - IntervalTreeNode* getSuccessor(IntervalTreeNode* node) const; - - /// @brief Return result for a given query - std::deque query(FCL_REAL low, FCL_REAL high); - - protected: - IntervalTreeNode* root; - - IntervalTreeNode* invalid_node; - - /// @brief left rotation of tree node - void leftRotate(IntervalTreeNode* node); - - /// @brief right rotation of tree node - void rightRotate(IntervalTreeNode* node); - - /// @brief Inserts node into the tree as if it were a regular binary tree - void recursiveInsert(IntervalTreeNode* node); - - /// @brief recursively print a subtree - void recursivePrint(IntervalTreeNode* node) const; - - /// @brief recursively find the node corresponding to the interval - IntervalTreeNode* recursiveSearch(IntervalTreeNode* node, - SimpleInterval* ivl) const; - - /// @brief Travels up to the root fixing the max_high fields after an - /// insertion or deletion - void fixupMaxHigh(IntervalTreeNode* node); - - void deleteFixup(IntervalTreeNode* node); - - private: - unsigned int recursion_node_stack_size; - it_recursion_node* recursion_node_stack; - unsigned int current_parent; - unsigned int recursion_node_stack_top; -}; - -} // namespace detail -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/interval_tree_node.h b/include/hpp/fcl/broadphase/detail/interval_tree_node.h index e8a653c15..531898a41 100644 --- a/include/hpp/fcl/broadphase/detail/interval_tree_node.h +++ b/include/hpp/fcl/broadphase/detail/interval_tree_node.h @@ -1,92 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_H -#define HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_H - -#include "hpp/fcl/broadphase/detail/simple_interval.h" -#include "hpp/fcl/fwd.hh" - -namespace hpp { -namespace fcl { - -namespace detail { - -class HPP_FCL_DLLAPI IntervalTree; - -/// @brief The node for interval tree -class HPP_FCL_DLLAPI IntervalTreeNode { - public: - friend class IntervalTree; - - /// @brief Create an empty node - IntervalTreeNode(); - - /// @brief Create an node storing the interval - IntervalTreeNode(SimpleInterval* new_interval); - - ~IntervalTreeNode(); - - /// @brief Print the interval node information: set left = invalid_node and - /// right = root - void print(IntervalTreeNode* left, IntervalTreeNode* right) const; - - protected: - /// @brief interval stored in the node - SimpleInterval* stored_interval; - - FCL_REAL key; - - FCL_REAL high; - - FCL_REAL max_high; - - /// @brief red or black node: if red = false then the node is black - bool red; - - IntervalTreeNode* left; - - IntervalTreeNode* right; - - IntervalTreeNode* parent; -}; - -} // namespace detail -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/morton-inl.h b/include/hpp/fcl/broadphase/detail/morton-inl.h index 53d7d90f7..b3ef57cba 100644 --- a/include/hpp/fcl/broadphase/detail/morton-inl.h +++ b/include/hpp/fcl/broadphase/detail/morton-inl.h @@ -1,153 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * Copyright (c) 2016, Toyota Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_MORTON_INL_H -#define HPP_FCL_MORTON_INL_H - -#include "hpp/fcl/broadphase/detail/morton.h" - -namespace hpp { -namespace fcl { /// @cond IGNORE -namespace detail { - -//============================================================================== -template -uint32_t quantize(S x, uint32_t n) { - return std::max(std::min((uint32_t)(x * (S)n), uint32_t(n - 1)), uint32_t(0)); -} - -//============================================================================== -template -morton_functor::morton_functor(const AABB& bbox) - : base(bbox.min_), - inv(1.0 / (bbox.max_[0] - bbox.min_[0]), - 1.0 / (bbox.max_[1] - bbox.min_[1]), - 1.0 / (bbox.max_[2] - bbox.min_[2])) { - // Do nothing -} - -//============================================================================== -template -uint32_t morton_functor::operator()(const Vec3f& point) const { - uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u); - uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u); - uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u); - - return detail::morton_code(x, y, z); -} - -//============================================================================== -template -morton_functor::morton_functor(const AABB& bbox) - : base(bbox.min_), - inv(1.0 / (bbox.max_[0] - bbox.min_[0]), - 1.0 / (bbox.max_[1] - bbox.min_[1]), - 1.0 / (bbox.max_[2] - bbox.min_[2])) { - // Do nothing -} - -//============================================================================== -template -uint64_t morton_functor::operator()(const Vec3f& point) const { - uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20); - uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20); - uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20); - - return detail::morton_code60(x, y, z); -} - -//============================================================================== -template -constexpr size_t morton_functor::bits() { - return 60; -} - -//============================================================================== -template -constexpr size_t morton_functor::bits() { - return 30; -} - -//============================================================================== -template -morton_functor>::morton_functor(const AABB& bbox) - : base(bbox.min_), - inv(1.0 / (bbox.max_[0] - bbox.min_[0]), - 1.0 / (bbox.max_[1] - bbox.min_[1]), - 1.0 / (bbox.max_[2] - bbox.min_[2])) { - // Do nothing -} - -//============================================================================== -template -std::bitset morton_functor>::operator()( - const Vec3f& point) const { - S x = (point[0] - base[0]) * inv[0]; - S y = (point[1] - base[1]) * inv[1]; - S z = (point[2] - base[2]) * inv[2]; - int start_bit = bits() - 1; - std::bitset bset; - - x *= 2; - y *= 2; - z *= 2; - - for (size_t i = 0; i < bits() / 3; ++i) { - bset[start_bit--] = ((z < 1) ? 0 : 1); - bset[start_bit--] = ((y < 1) ? 0 : 1); - bset[start_bit--] = ((x < 1) ? 0 : 1); - x = ((x >= 1) ? 2 * (x - 1) : 2 * x); - y = ((y >= 1) ? 2 * (y - 1) : 2 * y); - z = ((z >= 1) ? 2 * (z - 1) : 2 * z); - } - - return bset; -} - -//============================================================================== -template -constexpr size_t morton_functor>::bits() { - return N; -} - -} // namespace detail -/// @endcond -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/morton.h b/include/hpp/fcl/broadphase/detail/morton.h index cf9968af4..c0a89c6b2 100644 --- a/include/hpp/fcl/broadphase/detail/morton.h +++ b/include/hpp/fcl/broadphase/detail/morton.h @@ -1,122 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * Copyright (c) 2016, Toyota Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_MORTON_H -#define HPP_FCL_MORTON_H - -#include "hpp/fcl/BV/AABB.h" -#include -#include - -namespace hpp { -namespace fcl { - -/// @cond IGNORE -namespace detail { - -template -uint32_t quantize(S x, uint32_t n); - -/// @brief compute 30 bit morton code -HPP_FCL_DLLAPI -uint32_t morton_code(uint32_t x, uint32_t y, uint32_t z); - -/// @brief compute 60 bit morton code -HPP_FCL_DLLAPI -uint64_t morton_code60(uint32_t x, uint32_t y, uint32_t z); - -/// @brief Functor to compute the morton code for a given AABB -/// This is specialized for 32- and 64-bit unsigned integers giving -/// a 30- or 60-bit code, respectively, and for `std::bitset` where -/// N is the length of the code and must be a multiple of 3. -template -struct morton_functor {}; - -/// @brief Functor to compute 30 bit morton code for a given AABB -template -struct morton_functor { - morton_functor(const AABB& bbox); - - uint32_t operator()(const Vec3f& point) const; - - const Vec3f base; - const Vec3f inv; - - static constexpr size_t bits(); -}; - -using morton_functoru32f = morton_functor; -using morton_functoru32d = morton_functor; - -/// @brief Functor to compute 60 bit morton code for a given AABB -template -struct morton_functor { - morton_functor(const AABB& bbox); - - uint64_t operator()(const Vec3f& point) const; - - const Vec3f base; - const Vec3f inv; - - static constexpr size_t bits(); -}; - -/// @brief Functor to compute N bit morton code for a given AABB -/// N must be a multiple of 3. -template -struct morton_functor> { - static_assert(N % 3 == 0, "Number of bits must be a multiple of 3"); - - morton_functor(const AABB& bbox); - - std::bitset operator()(const Vec3f& point) const; - - const Vec3f base; - const Vec3f inv; - - static constexpr size_t bits(); -}; - -} // namespace detail -/// @endcond -} // namespace fcl -} // namespace hpp - -#include "hpp/fcl/broadphase/detail/morton-inl.h" - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/node_base-inl.h b/include/hpp/fcl/broadphase/detail/node_base-inl.h index d7a69680c..326020040 100644 --- a/include/hpp/fcl/broadphase/detail/node_base-inl.h +++ b/include/hpp/fcl/broadphase/detail/node_base-inl.h @@ -1,77 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_DETAIL_NODEBASE_INL_H -#define HPP_FCL_BROADPHASE_DETAIL_NODEBASE_INL_H - -#include "hpp/fcl/broadphase/detail/node_base.h" - -namespace hpp { -namespace fcl { -namespace detail { - -//============================================================================// -// // -// Implementations // -// // -//============================================================================// - -//============================================================================== -template -bool NodeBase::isLeaf() const { - return (children[1] == nullptr); -} - -//============================================================================== -template -bool NodeBase::isInternal() const { - return !isLeaf(); -} - -//============================================================================== -template -NodeBase::NodeBase() { - parent = nullptr; - children[0] = nullptr; - children[1] = nullptr; -} - -} // namespace detail -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/node_base.h b/include/hpp/fcl/broadphase/detail/node_base.h index 5937b9cef..b0c8206e3 100644 --- a/include/hpp/fcl/broadphase/detail/node_base.h +++ b/include/hpp/fcl/broadphase/detail/node_base.h @@ -1,81 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_DETAIL_NODEBASE_H -#define HPP_FCL_BROADPHASE_DETAIL_NODEBASE_H - -#include "hpp/fcl/data_types.h" - -namespace hpp { -namespace fcl { - -namespace detail { - -/// @brief dynamic AABB tree node -template -struct NodeBase { - /// @brief the bounding volume for the node - BV bv; - - /// @brief pointer to parent node - NodeBase* parent; - - /// @brief whether is a leaf - bool isLeaf() const; - - /// @brief whether is internal node - bool isInternal() const; - - union { - /// @brief for leaf node, children nodes - NodeBase* children[2]; - void* data; - }; - - /// @brief morton code for current BV - uint32_t code; - - NodeBase(); -}; - -} // namespace detail -} // namespace fcl -} // namespace hpp - -#include "hpp/fcl/broadphase/detail/node_base-inl.h" - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/node_base_array-inl.h b/include/hpp/fcl/broadphase/detail/node_base_array-inl.h index 0ee3e5f05..e66c59cdd 100644 --- a/include/hpp/fcl/broadphase/detail/node_base_array-inl.h +++ b/include/hpp/fcl/broadphase/detail/node_base_array-inl.h @@ -1,67 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H -#define HPP_FCL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H - -#include "hpp/fcl/broadphase/detail/node_base_array.h" - -namespace hpp { -namespace fcl { - -namespace detail { - -namespace implementation_array { - -//============================================================================== -template -bool NodeBase::isLeaf() const { - return (children[1] == (size_t)(-1)); -} - -//============================================================================== -template -bool NodeBase::isInternal() const { - return !isLeaf(); -} - -} // namespace implementation_array -} // namespace detail -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/node_base_array.h b/include/hpp/fcl/broadphase/detail/node_base_array.h index ecf8dc01d..8ad4c1ed3 100644 --- a/include/hpp/fcl/broadphase/detail/node_base_array.h +++ b/include/hpp/fcl/broadphase/detail/node_base_array.h @@ -1,77 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_DETAIL_NODEBASEARRAY_H -#define HPP_FCL_BROADPHASE_DETAIL_NODEBASEARRAY_H - -#include "hpp/fcl/data_types.h" - -namespace hpp { -namespace fcl { - -namespace detail { - -namespace implementation_array { - -template -struct NodeBase { - BV bv; - - union { - size_t parent; - size_t next; - }; - - union { - size_t children[2]; - void* data; - }; - - uint32_t code; - - bool isLeaf() const; - bool isInternal() const; -}; - -} // namespace implementation_array -} // namespace detail -} // namespace fcl -} // namespace hpp - -#include "hpp/fcl/broadphase/detail/node_base_array-inl.h" - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h b/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h index de51472a2..712c72ec3 100644 --- a/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h +++ b/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h @@ -1,113 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_INL_H -#define HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_INL_H - -#include "hpp/fcl/broadphase/detail/simple_hash_table.h" - -#include -namespace hpp { -namespace fcl { - -namespace detail { - -//============================================================================== -template -SimpleHashTable::SimpleHashTable(const HashFnc& h) : h_(h) { - // Do nothing -} - -//============================================================================== -template -void SimpleHashTable::init(size_t size) { - if (size == 0) { - HPP_FCL_THROW_PRETTY("SimpleHashTable must have non-zero size.", - std::logic_error); - } - - table_.resize(size); - table_size_ = size; -} - -//============================================================================== -template -void SimpleHashTable::insert(Key key, Data value) { - std::vector indices = h_(key); - size_t range = table_.size(); - for (size_t i = 0; i < indices.size(); ++i) - table_[indices[i] % range].push_back(value); -} - -//============================================================================== -template -std::vector SimpleHashTable::query(Key key) const { - size_t range = table_.size(); - std::vector indices = h_(key); - std::set result; - for (size_t i = 0; i < indices.size(); ++i) { - size_t index = indices[i] % range; - std::copy(table_[index].begin(), table_[index].end(), - std::inserter(result, result.end())); - } - - return std::vector(result.begin(), result.end()); -} - -//============================================================================== -template -void SimpleHashTable::remove(Key key, Data value) { - size_t range = table_.size(); - std::vector indices = h_(key); - for (size_t i = 0; i < indices.size(); ++i) { - size_t index = indices[i] % range; - table_[index].remove(value); - } -} - -//============================================================================== -template -void SimpleHashTable::clear() { - table_.clear(); - table_.resize(table_size_); -} - -} // namespace detail -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/simple_hash_table.h b/include/hpp/fcl/broadphase/detail/simple_hash_table.h index ed1f0fcd8..678cd34c8 100644 --- a/include/hpp/fcl/broadphase/detail/simple_hash_table.h +++ b/include/hpp/fcl/broadphase/detail/simple_hash_table.h @@ -1,89 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_H -#define HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_H - -#include -#include -#include - -namespace hpp { -namespace fcl { - -namespace detail { - -/// @brief A simple hash table implemented as multiple buckets. HashFnc is any -/// extended hash function: HashFnc(key) = {index1, index2, ..., } -template -class SimpleHashTable { - protected: - typedef std::list Bin; - - std::vector table_; - - HashFnc h_; - - size_t table_size_; - - public: - SimpleHashTable(const HashFnc& h); - - /// @brief Init the number of bins in the hash table - void init(size_t size); - - //// @brief Insert a key-value pair into the table - void insert(Key key, Data value); - - /// @brief Find the elements in the hash table whose key is the same as query - /// key. - std::vector query(Key key) const; - - /// @brief remove the key-value pair from the table - void remove(Key key, Data value); - - /// @brief clear the hash table - void clear(); -}; - -} // namespace detail -} // namespace fcl -} // namespace hpp - -#include "hpp/fcl/broadphase/detail/simple_hash_table-inl.h" - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/simple_interval-inl.h b/include/hpp/fcl/broadphase/detail/simple_interval-inl.h index 78719156c..b6e1fbd6c 100644 --- a/include/hpp/fcl/broadphase/detail/simple_interval-inl.h +++ b/include/hpp/fcl/broadphase/detail/simple_interval-inl.h @@ -1,62 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H -#define HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H - -#include "hpp/fcl/broadphase/detail/simple_interval.h" -#include - -namespace hpp { -namespace fcl { -namespace detail { - -//============================================================================== -SimpleInterval::~SimpleInterval() { - // Do nothing -} - -//============================================================================== -void SimpleInterval::print() { - // Do nothing -} - -} // namespace detail -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/simple_interval.h b/include/hpp/fcl/broadphase/detail/simple_interval.h index 0f1976ebf..f0c26e038 100644 --- a/include/hpp/fcl/broadphase/detail/simple_interval.h +++ b/include/hpp/fcl/broadphase/detail/simple_interval.h @@ -1,64 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H -#define HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H - -#include "hpp/fcl/fwd.hh" -#include "hpp/fcl/data_types.h" - -namespace hpp { -namespace fcl { -namespace detail { - -/// @brief Interval trees implemented using red-black-trees as described in -/// the book Introduction_To_Algorithms_ by Cormen, Leisserson, and Rivest. -struct HPP_FCL_DLLAPI SimpleInterval { - public: - virtual ~SimpleInterval(); - - virtual void print(); - - /// @brief interval is defined as [low, high] - FCL_REAL low, high; -}; - -} // namespace detail -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h b/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h index b0668d989..cabe7e8fb 100644 --- a/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h +++ b/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h @@ -1,113 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_SPARSEHASHTABLE_INL_H -#define HPP_FCL_BROADPHASE_SPARSEHASHTABLE_INL_H - -#include "hpp/fcl/broadphase/detail/sparse_hash_table.h" - -namespace hpp { -namespace fcl { - -namespace detail { - -//============================================================================== -template class TableT> -SparseHashTable::SparseHashTable(const HashFnc& h) - : h_(h) { - // Do nothing -} - -//============================================================================== -template class TableT> -void SparseHashTable::init(size_t) { - table_.clear(); -} - -//============================================================================== -template class TableT> -void SparseHashTable::insert(Key key, Data value) { - std::vector indices = h_(key); - for (size_t i = 0; i < indices.size(); ++i) - table_[indices[i]].push_back(value); -} - -//============================================================================== -template class TableT> -std::vector SparseHashTable::query( - Key key) const { - std::vector indices = h_(key); - std::set result; - for (size_t i = 0; i < indices.size(); ++i) { - unsigned int index = indices[i]; - typename Table::const_iterator p = table_.find(index); - if (p != table_.end()) { - std::copy((*p).second.begin(), (*p).second.end(), - std ::inserter(result, result.end())); - } - } - - return std::vector(result.begin(), result.end()); -} - -//============================================================================== -template class TableT> -void SparseHashTable::remove(Key key, Data value) { - std::vector indices = h_(key); - for (size_t i = 0; i < indices.size(); ++i) { - unsigned int index = indices[i]; - table_[index].remove(value); - } -} - -//============================================================================== -template class TableT> -void SparseHashTable::clear() { - table_.clear(); -} - -} // namespace detail -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/sparse_hash_table.h b/include/hpp/fcl/broadphase/detail/sparse_hash_table.h index 5c28ccf3f..9f585fac5 100644 --- a/include/hpp/fcl/broadphase/detail/sparse_hash_table.h +++ b/include/hpp/fcl/broadphase/detail/sparse_hash_table.h @@ -1,95 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_SPARSEHASHTABLE_H -#define HPP_FCL_BROADPHASE_SPARSEHASHTABLE_H - -#include -#include -#include -#include - -namespace hpp { -namespace fcl { - -namespace detail { - -template -class unordered_map_hash_table : public std::unordered_map { - typedef std::unordered_map Base; - - public: - unordered_map_hash_table() : Base() {}; -}; - -/// @brief A hash table implemented using unordered_map -template class TableT = unordered_map_hash_table> -class SparseHashTable { - protected: - HashFnc h_; - typedef std::list Bin; - typedef TableT Table; - - Table table_; - - public: - SparseHashTable(const HashFnc& h); - - /// @brief Init the hash table. The bucket size is dynamically decided. - void init(size_t); - - /// @brief insert one key-value pair into the hash table - void insert(Key key, Data value); - - /// @brief find the elements whose key is the same as the query - std::vector query(Key key) const; - - /// @brief remove one key-value pair from the hash table - void remove(Key key, Data value); - - /// @brief clear the hash table - void clear(); -}; - -} // namespace detail -} // namespace fcl -} // namespace hpp - -#include "hpp/fcl/broadphase/detail/sparse_hash_table-inl.h" - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h b/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h index 1248b9eae..be5a1d873 100644 --- a/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h +++ b/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h @@ -1,82 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_SPATIALHASH_INL_H -#define HPP_FCL_BROADPHASE_SPATIALHASH_INL_H - -#include "hpp/fcl/broadphase/detail/spatial_hash.h" -#include - -namespace hpp { -namespace fcl { -namespace detail { - -//============================================================================== -SpatialHash::SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_) - : cell_size(cell_size_), scene_limit(scene_limit_) { - width[0] = std::ceil(scene_limit.width() / cell_size); - width[1] = std::ceil(scene_limit.height() / cell_size); - width[2] = std::ceil(scene_limit.depth() / cell_size); -} - -//============================================================================== -std::vector SpatialHash::operator()(const AABB& aabb) const { - int min_x = std::floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size); - int max_x = std::ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size); - int min_y = std::floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size); - int max_y = std::ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size); - int min_z = std::floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size); - int max_z = std::ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size); - - std::vector keys((max_x - min_x) * (max_y - min_y) * - (max_z - min_z)); - int id = 0; - for (int x = min_x; x < max_x; ++x) { - for (int y = min_y; y < max_y; ++y) { - for (int z = min_z; z < max_z; ++z) { - keys[id++] = x + y * width[0] + z * width[0] * width[1]; - } - } - } - return keys; -} - -} // namespace detail -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/spatial_hash.h b/include/hpp/fcl/broadphase/detail/spatial_hash.h index a7b711ea2..f4f10af83 100644 --- a/include/hpp/fcl/broadphase/detail/spatial_hash.h +++ b/include/hpp/fcl/broadphase/detail/spatial_hash.h @@ -1,65 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_BROADPHASE_SPATIALHASH_H -#define HPP_FCL_BROADPHASE_SPATIALHASH_H - -#include "hpp/fcl/BV/AABB.h" -#include - -namespace hpp { -namespace fcl { - -namespace detail { - -/// @brief Spatial hash function: hash an AABB to a set of integer values -struct HPP_FCL_DLLAPI SpatialHash { - SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_); - - std::vector operator()(const AABB& aabb) const; - - private: - FCL_REAL cell_size; - AABB scene_limit; - unsigned int width[3]; -}; - -} // namespace detail -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/coal.hpp b/include/hpp/fcl/coal.hpp new file mode 100644 index 000000000..e4fdb758c --- /dev/null +++ b/include/hpp/fcl/coal.hpp @@ -0,0 +1,19 @@ +#ifndef HPP_FCL_COAL_HPP +#define HPP_FCL_COAL_HPP + +#include +#include + +#define COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL + +#ifdef _MSC_VER +#pragma message COAL_DEPRECATED_HEADER( \ + "Please update your includes from 'hpp/fcl' to 'coal'") +#else +#warning "Please update your includes from 'hpp/fcl' to 'coal'" +#endif + +#define HPP_FCL_VERSION_AT_LEAST(major, minor, patch) \ + COAL_VERSION_AT_LEAST(major, minor, patch) + +#endif // COAL_FWD_HPP diff --git a/include/hpp/fcl/collision.h b/include/hpp/fcl/collision.h index cbc0bd562..099670f25 100644 --- a/include/hpp/fcl/collision.h +++ b/include/hpp/fcl/collision.h @@ -1,122 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * Copyright (c) 2021, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_COLLISION_H -#define HPP_FCL_COLLISION_H - -#include -#include -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// @brief Main collision interface: given two collision objects, and the -/// requirements for contacts, including num of max contacts, whether perform -/// exhaustive collision (i.e., returning returning all the contact points), -/// whether return detailed contact information (i.e., normal, contact point, -/// depth; otherwise only contact primitive id is returned), this function -/// performs the collision between them. -/// Return value is the number of contacts generated between the two objects. -HPP_FCL_DLLAPI std::size_t collide(const CollisionObject* o1, - const CollisionObject* o2, - const CollisionRequest& request, - CollisionResult& result); - -/// @copydoc collide(const CollisionObject*, const CollisionObject*, const -/// CollisionRequest&, CollisionResult&) -HPP_FCL_DLLAPI std::size_t collide(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const CollisionRequest& request, - CollisionResult& result); - -/// @brief This class reduces the cost of identifying the geometry pair. -/// This is mostly useful for repeated shape-shape queries. -/// -/// \code -/// ComputeCollision calc_collision (o1, o2); -/// std::size_t ncontacts = calc_collision(tf1, tf2, request, result); -/// \endcode -class HPP_FCL_DLLAPI ComputeCollision { - public: - /// @brief Default constructor from two Collision Geometries. - ComputeCollision(const CollisionGeometry* o1, const CollisionGeometry* o2); - - std::size_t operator()(const Transform3f& tf1, const Transform3f& tf2, - const CollisionRequest& request, - CollisionResult& result) const; - - bool operator==(const ComputeCollision& other) const { - return o1 == other.o1 && o2 == other.o2 && solver == other.solver; - } - - bool operator!=(const ComputeCollision& other) const { - return !(*this == other); - } - - virtual ~ComputeCollision() {}; - - protected: - // These pointers are made mutable to let the derived classes to update - // their values when updating the collision geometry (e.g. creating a new - // one). This feature should be used carefully to avoid any mis usage (e.g, - // changing the type of the collision geometry should be avoided). - mutable const CollisionGeometry* o1; - mutable const CollisionGeometry* o2; - - mutable GJKSolver solver; - - CollisionFunctionMatrix::CollisionFunc func; - bool swap_geoms; - - virtual std::size_t run(const Transform3f& tf1, const Transform3f& tf2, - const CollisionRequest& request, - CollisionResult& result) const; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h index ee03e35b1..185424ac7 100644 --- a/include/hpp/fcl/collision_data.h +++ b/include/hpp/fcl/collision_data.h @@ -1,1241 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * Copyright (c) 2024, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_COLLISION_DATA_H -#define HPP_FCL_COLLISION_DATA_H - -#include -#include -#include -#include -#include - -#include "hpp/fcl/collision_object.h" -#include "hpp/fcl/config.hh" -#include "hpp/fcl/data_types.h" -#include "hpp/fcl/timings.h" -#include "hpp/fcl/narrowphase/narrowphase_defaults.h" -#include "hpp/fcl/logging.h" - -namespace hpp { -namespace fcl { - -/// @brief Contact information returned by collision -struct HPP_FCL_DLLAPI Contact { - /// @brief collision object 1 - const CollisionGeometry* o1; - - /// @brief collision object 2 - const CollisionGeometry* o2; - - /// @brief contact primitive in object 1 - /// if object 1 is mesh or point cloud, it is the triangle or point id - /// if object 1 is geometry shape, it is NONE (-1), - /// if object 1 is octree, it is the id of the cell - int b1; - - /// @brief contact primitive in object 2 - /// if object 2 is mesh or point cloud, it is the triangle or point id - /// if object 2 is geometry shape, it is NONE (-1), - /// if object 2 is octree, it is the id of the cell - int b2; - - /// @brief contact normal, pointing from o1 to o2. - /// The normal defined as the normalized separation vector: - /// normal = (p2 - p1) / dist(o1, o2), where p1 = nearest_points[0] - /// belongs to o1 and p2 = nearest_points[1] belongs to o2 and dist(o1, o2) is - /// the **signed** distance between o1 and o2. The normal always points from - /// o1 to o2. - /// @note The separation vector is the smallest vector such that if o1 is - /// translated by it, o1 and o2 are in touching contact (they share at least - /// one contact point but have a zero intersection volume). If the shapes - /// overlap, dist(o1, o2) = -((p2-p1).norm()). Otherwise, dist(o1, o2) = - /// (p2-p1).norm(). - Vec3f normal; - - /// @brief nearest points associated to this contact. - /// @note Also referred as "witness points" in other collision libraries. - /// The points p1 = nearest_points[0] and p2 = nearest_points[1] verify the - /// property that dist(o1, o2) * (p1 - p2) is the separation vector between o1 - /// and o2, with dist(o1, o2) being the **signed** distance separating o1 from - /// o2. See \ref DistanceResult::normal for the definition of the separation - /// vector. If o1 and o2 have multiple contacts, the nearest_points are - /// associated with the contact which has the greatest penetration depth. - /// TODO (louis): rename `nearest_points` to `witness_points`. - std::array nearest_points; - - /// @brief contact position, in world space - Vec3f pos; - - /// @brief penetration depth - FCL_REAL penetration_depth; - - /// @brief invalid contact primitive information - static const int NONE = -1; - - /// @brief Default constructor - Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) { - penetration_depth = (std::numeric_limits::max)(); - nearest_points[0] = nearest_points[1] = normal = pos = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); - } - - Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, - int b2_) - : o1(o1_), o2(o2_), b1(b1_), b2(b2_) { - penetration_depth = (std::numeric_limits::max)(); - nearest_points[0] = nearest_points[1] = normal = pos = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); - } - - Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, - int b2_, const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_) - : o1(o1_), - o2(o2_), - b1(b1_), - b2(b2_), - normal(normal_), - nearest_points{pos_ - (depth_ * normal_ / 2), - pos_ + (depth_ * normal_ / 2)}, - pos(pos_), - penetration_depth(depth_) {} - - Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, - int b2_, const Vec3f& p1, const Vec3f& p2, const Vec3f& normal_, - FCL_REAL depth_) - : o1(o1_), - o2(o2_), - b1(b1_), - b2(b2_), - normal(normal_), - nearest_points{p1, p2}, - pos((p1 + p2) / 2), - penetration_depth(depth_) {} - - bool operator<(const Contact& other) const { - if (b1 == other.b1) return b2 < other.b2; - return b1 < other.b1; - } - - bool operator==(const Contact& other) const { - return o1 == other.o1 && o2 == other.o2 && b1 == other.b1 && - b2 == other.b2 && normal == other.normal && pos == other.pos && - nearest_points[0] == other.nearest_points[0] && - nearest_points[1] == other.nearest_points[1] && - penetration_depth == other.penetration_depth; - } - - bool operator!=(const Contact& other) const { return !(*this == other); } - - FCL_REAL getDistanceToCollision(const CollisionRequest& request) const; -}; - -struct QueryResult; - -/// @brief base class for all query requests -struct HPP_FCL_DLLAPI QueryRequest { - // @brief Initial guess to use for the GJK algorithm - GJKInitialGuess gjk_initial_guess; - - /// @brief whether enable gjk initial guess - /// @Deprecated Use gjk_initial_guess instead - HPP_FCL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead) - bool enable_cached_gjk_guess; - - /// @brief the gjk initial guess set by user - mutable Vec3f cached_gjk_guess; - - /// @brief the support function initial guess set by user - mutable support_func_guess_t cached_support_func_guess; - - /// @brief maximum iteration for the GJK algorithm - size_t gjk_max_iterations; - - /// @brief tolerance for the GJK algorithm. - /// Note: This tolerance determines the precision on the estimated distance - /// between two geometries which are not in collision. - /// It is recommended to not set this tolerance to less than 1e-6. - FCL_REAL gjk_tolerance; - - /// @brief whether to enable the Nesterov accleration of GJK - GJKVariant gjk_variant; - - /// @brief convergence criterion used to stop GJK - GJKConvergenceCriterion gjk_convergence_criterion; - - /// @brief convergence criterion used to stop GJK - GJKConvergenceCriterionType gjk_convergence_criterion_type; - - /// @brief max number of iterations for EPA - size_t epa_max_iterations; - - /// @brief tolerance for EPA. - /// Note: This tolerance determines the precision on the estimated distance - /// between two geometries which are in collision. - /// It is recommended to not set this tolerance to less than 1e-6. - /// Also, setting EPA's tolerance to less than GJK's is not recommended. - FCL_REAL epa_tolerance; - - /// @brief enable timings when performing collision/distance request - bool enable_timings; - - /// @brief threshold below which a collision is considered. - FCL_REAL collision_distance_threshold; - - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - /// @brief Default constructor. - QueryRequest() - : gjk_initial_guess(GJKInitialGuess::DefaultGuess), - enable_cached_gjk_guess(false), - cached_gjk_guess(1, 0, 0), - cached_support_func_guess(support_func_guess_t::Zero()), - gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS), - gjk_tolerance(GJK_DEFAULT_TOLERANCE), - gjk_variant(GJKVariant::DefaultGJK), - gjk_convergence_criterion(GJKConvergenceCriterion::Default), - gjk_convergence_criterion_type(GJKConvergenceCriterionType::Relative), - epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS), - epa_tolerance(EPA_DEFAULT_TOLERANCE), - enable_timings(false), - collision_distance_threshold( - Eigen::NumTraits::dummy_precision()) {} - - /// @brief Copy constructor. - QueryRequest(const QueryRequest& other) = default; - - /// @brief Copy assignment operator. - QueryRequest& operator=(const QueryRequest& other) = default; - HPP_FCL_COMPILER_DIAGNOSTIC_POP - - /// @brief Updates the guess for the internal GJK algorithm in order to - /// warm-start it when reusing this collision request on the same collision - /// pair. - /// @note The option `gjk_initial_guess` must be set to - /// `GJKInitialGuess::CachedGuess` for this to work. - void updateGuess(const QueryResult& result) const; - - /// @brief whether two QueryRequest are the same or not - inline bool operator==(const QueryRequest& other) const { - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - return gjk_initial_guess == other.gjk_initial_guess && - enable_cached_gjk_guess == other.enable_cached_gjk_guess && - gjk_variant == other.gjk_variant && - gjk_convergence_criterion == other.gjk_convergence_criterion && - gjk_convergence_criterion_type == - other.gjk_convergence_criterion_type && - gjk_tolerance == other.gjk_tolerance && - gjk_max_iterations == other.gjk_max_iterations && - cached_gjk_guess == other.cached_gjk_guess && - cached_support_func_guess == other.cached_support_func_guess && - epa_max_iterations == other.epa_max_iterations && - epa_tolerance == other.epa_tolerance && - enable_timings == other.enable_timings && - collision_distance_threshold == other.collision_distance_threshold; - HPP_FCL_COMPILER_DIAGNOSTIC_POP - } -}; - -/// @brief base class for all query results -struct HPP_FCL_DLLAPI QueryResult { - /// @brief stores the last GJK ray when relevant. - Vec3f cached_gjk_guess; - - /// @brief stores the last support function vertex index, when relevant. - support_func_guess_t cached_support_func_guess; - - /// @brief timings for the given request - CPUTimes timings; - - QueryResult() - : cached_gjk_guess(Vec3f::Zero()), - cached_support_func_guess(support_func_guess_t::Constant(-1)) {} -}; - -inline void QueryRequest::updateGuess(const QueryResult& result) const { - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - if (gjk_initial_guess == GJKInitialGuess::CachedGuess || - enable_cached_gjk_guess) { - cached_gjk_guess = result.cached_gjk_guess; - cached_support_func_guess = result.cached_support_func_guess; - } - HPP_FCL_COMPILER_DIAGNOSTIC_POP -} - -struct CollisionResult; - -/// @brief flag declaration for specifying required params in CollisionResult -enum CollisionRequestFlag { - CONTACT = 0x00001, - DISTANCE_LOWER_BOUND = 0x00002, - NO_REQUEST = 0x01000 -}; - -/// @brief request to the collision algorithm -struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest { - /// @brief The maximum number of contacts that can be returned - size_t num_max_contacts; - - /// @brief whether the contact information (normal, penetration depth and - /// contact position) will return. - bool enable_contact; - - /// Whether a lower bound on distance is returned when objects are disjoint - HPP_FCL_DEPRECATED_MESSAGE(A lower bound on distance is always computed.) - bool enable_distance_lower_bound; - - /// @brief Distance below which objects are considered in collision. - /// See \ref hpp_fcl_collision_and_distance_lower_bound_computation - /// @note If set to -inf, the objects tested for collision are considered - /// as collision free and no test is actually performed by functions - /// hpp::fcl::collide of class hpp::fcl::ComputeCollision. - FCL_REAL security_margin; - - /// @brief Distance below which bounding volumes are broken down. - /// See \ref hpp_fcl_collision_and_distance_lower_bound_computation - FCL_REAL break_distance; - - /// @brief Distance above which GJK solver makes an early stopping. - /// GJK stops searching for the closest points when it proves that the - /// distance between two geometries is above this threshold. - /// - /// @remarks Consequently, the closest points might be incorrect, but allows - /// to save computational resources. - FCL_REAL distance_upper_bound; - - /// @brief Constructor from a flag and a maximal number of contacts. - /// - /// @param[in] flag Collision request flag - /// @param[in] num_max_contacts Maximal number of allowed contacts - /// - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_) - : num_max_contacts(num_max_contacts_), - enable_contact(flag & CONTACT), - enable_distance_lower_bound(flag & DISTANCE_LOWER_BOUND), - security_margin(0), - break_distance(1e-3), - distance_upper_bound((std::numeric_limits::max)()) {} - - /// @brief Default constructor. - CollisionRequest() - : num_max_contacts(1), - enable_contact(true), - enable_distance_lower_bound(false), - security_margin(0), - break_distance(1e-3), - distance_upper_bound((std::numeric_limits::max)()) {} - HPP_FCL_COMPILER_DIAGNOSTIC_POP - - bool isSatisfied(const CollisionResult& result) const; - - /// @brief whether two CollisionRequest are the same or not - inline bool operator==(const CollisionRequest& other) const { - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - return QueryRequest::operator==(other) && - num_max_contacts == other.num_max_contacts && - enable_contact == other.enable_contact && - enable_distance_lower_bound == other.enable_distance_lower_bound && - security_margin == other.security_margin && - break_distance == other.break_distance && - distance_upper_bound == other.distance_upper_bound; - HPP_FCL_COMPILER_DIAGNOSTIC_POP - } -}; - -inline FCL_REAL Contact::getDistanceToCollision( - const CollisionRequest& request) const { - return penetration_depth - request.security_margin; -} - -/// @brief collision result -struct HPP_FCL_DLLAPI CollisionResult : QueryResult { - private: - /// @brief contact information - std::vector contacts; - - public: - /// Lower bound on distance between objects if they are disjoint. - /// See \ref hpp_fcl_collision_and_distance_lower_bound_computation - /// @note Always computed. If \ref CollisionRequest::distance_upper_bound is - /// set to infinity, distance_lower_bound is the actual distance between the - /// shapes. - FCL_REAL distance_lower_bound; - - /// @brief normal associated to nearest_points. - /// Same as `CollisionResult::nearest_points` but for the normal. - Vec3f normal; - - /// @brief nearest points. - /// A `CollisionResult` can have multiple contacts. - /// The nearest points in `CollisionResults` correspond to the witness points - /// associated with the smallest distance i.e the `distance_lower_bound`. - /// For bounding volumes and BVHs, these nearest points are available - /// only when distance_lower_bound is inferior to - /// CollisionRequest::break_distance. - std::array nearest_points; - - public: - CollisionResult() - : distance_lower_bound((std::numeric_limits::max)()) { - nearest_points[0] = nearest_points[1] = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); - } - - /// @brief Update the lower bound only if the distance is inferior. - inline void updateDistanceLowerBound(const FCL_REAL& distance_lower_bound_) { - if (distance_lower_bound_ < distance_lower_bound) - distance_lower_bound = distance_lower_bound_; - } - - /// @brief add one contact into result structure - inline void addContact(const Contact& c) { contacts.push_back(c); } - - /// @brief whether two CollisionResult are the same or not - inline bool operator==(const CollisionResult& other) const { - return contacts == other.contacts && - distance_lower_bound == other.distance_lower_bound && - nearest_points[0] == other.nearest_points[0] && - nearest_points[1] == other.nearest_points[1] && - normal == other.normal; - } - - /// @brief return binary collision result - bool isCollision() const { return contacts.size() > 0; } - - /// @brief number of contacts found - size_t numContacts() const { return contacts.size(); } - - /// @brief get the i-th contact calculated - const Contact& getContact(size_t i) const { - if (contacts.size() == 0) - HPP_FCL_THROW_PRETTY( - "The number of contacts is zero. No Contact can be returned.", - std::invalid_argument); - - if (i < contacts.size()) - return contacts[i]; - else - return contacts.back(); - } - - /// @brief set the i-th contact calculated - void setContact(size_t i, const Contact& c) { - if (contacts.size() == 0) - HPP_FCL_THROW_PRETTY( - "The number of contacts is zero. No Contact can be returned.", - std::invalid_argument); - - if (i < contacts.size()) - contacts[i] = c; - else - contacts.back() = c; - } - - /// @brief get all the contacts - void getContacts(std::vector& contacts_) const { - contacts_.resize(contacts.size()); - std::copy(contacts.begin(), contacts.end(), contacts_.begin()); - } - - const std::vector& getContacts() const { return contacts; } - - /// @brief clear the results obtained - void clear() { - distance_lower_bound = (std::numeric_limits::max)(); - contacts.clear(); - timings.clear(); - nearest_points[0] = nearest_points[1] = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); - } - - /// @brief reposition Contact objects when fcl inverts them - /// during their construction. - void swapObjects(); -}; - -/// @brief This structure allows to encode contact patches. -/// A contact patch is defined by a set of points belonging to a subset of a -/// plane passing by `p` and supported by `n`, where `n = Contact::normal` and -/// `p = Contact::pos`. If we denote by P this plane and by S1 and S2 the first -/// and second shape of a collision pair, a contact patch is represented as a -/// polytope which vertices all belong to `P & S1 & S2`, where `&` denotes the -/// set-intersection. Since a contact patch is a subset of a plane supported by -/// `n`, it has a preferred direction. In HPP-FCL, the `Contact::normal` points -/// from S1 to S2. In the same way, a contact patch points by default from S1 -/// to S2. -/// -/// @note For now (April 2024), a `ContactPatch` is a polygon (2D polytope), -/// so the points of the set, forming the convex-hull of the polytope, are -/// stored in a counter-clockwise fashion. -/// @note If needed, the internal algorithms of hpp-fcl can easily be extended -/// to compute a contact volume instead of a contact patch. -struct HPP_FCL_DLLAPI ContactPatch { - public: - using Polygon = std::vector; - - /// @brief Frame of the set, expressed in the world coordinates. - /// The z-axis of the frame's rotation is the contact patch normal. - Transform3f tf; - - /// @brief Direction of ContactPatch. - /// When doing collision detection, the convention of HPP-FCL is that the - /// normal always points from the first to the second shape of the collision - /// pair i.e. from shape1 to shape2 when calling `collide(shape1, shape2)`. - /// The PatchDirection enum allows to identify if the patch points from - /// shape 1 to shape 2 (Default type) or from shape 2 to shape 1 (Inverted - /// type). The Inverted type should only be used for internal HPP-FCL - /// computations (it allows to properly define two separate contact patches in - /// the same frame). - enum PatchDirection { DEFAULT = 0, INVERTED = 1 }; - - /// @brief Direction of this contact patch. - PatchDirection direction; - - /// @brief Penetration depth of the contact patch. This value corresponds to - /// the signed distance `d` between the shapes. - /// @note For each contact point `p` in the patch of normal `n`, `p1 = p - - /// 0.5*d*n` and `p2 = p + 0.5*d*n` define a pair of witness points. `p1` - /// belongs to the surface of the first shape and `p2` belongs to the surface - /// of the second shape. For any pair of witness points, we always have `p2 - - /// p1 = d * n`. The vector `d * n` is called a minimum separation vector: - /// if S1 is translated by it, S1 and S2 are not in collision anymore. - /// @note Although there may exist multiple minimum separation vectors between - /// two shapes, the term "minimum" comes from the fact that it's impossible to - /// find a different separation vector which has a smaller norm than `d * n`. - FCL_REAL penetration_depth; - - /// @brief Default maximum size of the polygon representing the contact patch. - /// Used to pre-allocate memory for the patch. - static constexpr size_t default_preallocated_size = 12; - - protected: - /// @brief Container for the vertices of the set. - Polygon m_points; - - public: - /// @brief Default constructor. - /// Note: the preallocated size does not determine the maximum number of - /// points in the patch, it only serves as preallocation if the maximum size - /// of the patch is known in advance. HPP-FCL will automatically expand/shrink - /// the contact patch if needed. - explicit ContactPatch(size_t preallocated_size = default_preallocated_size) - : tf(Transform3f::Identity()), - direction(PatchDirection::DEFAULT), - penetration_depth(0) { - this->m_points.reserve(preallocated_size); - } - - /// @brief Normal of the contact patch, expressed in the WORLD frame. - Vec3f getNormal() const { - if (this->direction == PatchDirection::INVERTED) { - return -this->tf.rotation().col(2); - } - return this->tf.rotation().col(2); - } - - /// @brief Returns the number of points in the contact patch. - size_t size() const { return this->m_points.size(); } - - /// @brief Add a 3D point to the set, expressed in the world frame. - /// @note This function takes a 3D point and expresses it in the local frame - /// of the set. It then takes only the x and y components of the vector, - /// effectively doing a projection onto the plane to which the set belongs. - /// TODO(louis): if necessary, we can store the offset to the plane (x, y). - void addPoint(const Vec3f& point_3d) { - const Vec3f point = this->tf.inverseTransform(point_3d); - this->m_points.emplace_back(point.template head<2>()); - } - - /// @brief Get the i-th point of the set, expressed in the 3D world frame. - Vec3f getPoint(const size_t i) const { - Vec3f point(0, 0, 0); - point.head<2>() = this->point(i); - point = tf.transform(point); - return point; - } - - /// @brief Get the i-th point of the contact patch, projected back onto the - /// first shape of the collision pair. This point is expressed in the 3D - /// world frame. - Vec3f getPointShape1(const size_t i) const { - Vec3f point = this->getPoint(i); - point -= (this->penetration_depth / 2) * this->getNormal(); - return point; - } - - /// @brief Get the i-th point of the contact patch, projected back onto the - /// first shape of the collision pair. This 3D point is expressed in the world - /// frame. - Vec3f getPointShape2(const size_t i) const { - Vec3f point = this->getPoint(i); - point += (this->penetration_depth / 2) * this->getNormal(); - return point; - } - - /// @brief Getter for the 2D points in the set. - Polygon& points() { return this->m_points; } - - /// @brief Const getter for the 2D points in the set. - const Polygon& points() const { return this->m_points; } - - /// @brief Getter for the i-th 2D point in the set. - Vec2f& point(const size_t i) { - HPP_FCL_ASSERT(this->m_points.size() > 0, "Patch is empty.", - std::logic_error); - if (i < this->m_points.size()) { - return this->m_points[i]; - } - return this->m_points.back(); - } - - /// @brief Const getter for the i-th 2D point in the set. - const Vec2f& point(const size_t i) const { - HPP_FCL_ASSERT(this->m_points.size() > 0, "Patch is empty.", - std::logic_error); - if (i < this->m_points.size()) { - return this->m_points[i]; - } - return this->m_points.back(); - } - - /// @brief Clear the set. - void clear() { - this->m_points.clear(); - this->tf.setIdentity(); - this->penetration_depth = 0; - } - - /// @brief Whether two contact patches are the same or not. - /// @note This compares the two sets terms by terms. - /// However, two contact patches can be identical, but have a different - /// order for their points. Use `isEqual` in this case. - bool operator==(const ContactPatch& other) const { - return this->tf == other.tf && this->direction == other.direction && - this->penetration_depth == other.penetration_depth && - this->points() == other.points(); - } - - /// @brief Whether two contact patches are the same or not. - /// Checks for different order of the points. - bool isSame(const ContactPatch& other, - const FCL_REAL tol = - Eigen::NumTraits::dummy_precision()) const { - // The x and y axis of the set are arbitrary, but the z axis is - // always the normal. The position of the origin of the frame is also - // arbitrary. So we only check if the normals are the same. - if (!this->getNormal().isApprox(other.getNormal(), tol)) { - return false; - } - - if (std::abs(this->penetration_depth - other.penetration_depth) > tol) { - return false; - } - - if (this->direction != other.direction) { - return false; - } - - if (this->size() != other.size()) { - return false; - } - - // Check all points of the contact patch. - for (size_t i = 0; i < this->size(); ++i) { - bool found = false; - const Vec3f pi = this->getPoint(i); - for (size_t j = 0; j < other.size(); ++j) { - const Vec3f other_pj = other.getPoint(j); - if (pi.isApprox(other_pj, tol)) { - found = true; - } - } - if (!found) { - return false; - } - } - - return true; - } -}; - -/// @brief Construct a frame from a `Contact`'s position and normal. -/// Because both `Contact`'s position and normal are expressed in the world -/// frame, this frame is also expressed w.r.t the world frame. -/// The origin of the frame is `contact.pos` and the z-axis of the frame is -/// `contact.normal`. -inline void constructContactPatchFrameFromContact(const Contact& contact, - ContactPatch& contact_patch) { - contact_patch.penetration_depth = contact.penetration_depth; - contact_patch.tf.translation() = contact.pos; - contact_patch.tf.rotation() = - constructOrthonormalBasisFromVector(contact.normal); - contact_patch.direction = ContactPatch::PatchDirection::DEFAULT; -} - -/// @brief Structure used for internal computations. A support set and a -/// contact patch can be represented by the same structure. In fact, a contact -/// patch is the intersection of two support sets, one with -/// `PatchDirection::DEFAULT` and one with `PatchDirection::INVERTED`. -/// @note A support set with `DEFAULT` direction is the support set of a shape -/// in the direction given by `+n`, where `n` is the z-axis of the frame's -/// patch rotation. An `INVERTED` support set is the support set of a shape in -/// the direction `-n`. -using SupportSet = ContactPatch; - -/// @brief Request for a contact patch computation. -struct HPP_FCL_DLLAPI ContactPatchRequest { - /// @brief Maximum number of contact patches that will be computed. - size_t max_num_patch; - - protected: - /// @brief Maximum samples to compute the support sets of curved shapes, - /// i.e. when the normal is perpendicular to the base of a cylinder. For - /// now, only relevant for Cone and Cylinder. In the future this might be - /// extended to Sphere and Ellipsoid. - size_t m_num_samples_curved_shapes; - - /// @brief Tolerance below which points are added to a contact patch. - /// In details, given two shapes S1 and S2, a contact patch is the triple - /// intersection between the separating plane (P) (passing by `Contact::pos` - /// and supported by `Contact::normal`), S1 and S2; i.e. a contact patch is - /// `P & S1 & S2` if we denote `&` the set intersection operator. If a point - /// p1 of S1 is at a distance below `patch_tolerance` from the separating - /// plane, it is taken into account in the computation of the contact patch. - /// Otherwise, it is not used for the computation. - /// @note Needs to be positive. - FCL_REAL m_patch_tolerance; - - public: - /// @brief Default constructor. - /// @param max_num_patch maximum number of contact patches per collision pair. - /// @param max_sub_patch_size maximum size of each sub contact patch. Each - /// contact patch contains an internal representation for an inscribed sub - /// contact patch. This allows physics simulation to always work with a - /// predetermined maximum size for each contact patch. A sub contact patch is - /// simply a subset of the vertices of a contact patch. - /// @param num_samples_curved_shapes for shapes like cones and cylinders, - /// which have smooth basis (circles in this case), we need to sample a - /// certain amount of point of this basis. - /// @param patch_tolerance the tolerance below which a point of a shape is - /// considered to belong to the support set of this shape in the direction of - /// the normal. Said otherwise, `patch_tolerance` determines the "thickness" - /// of the separating plane between shapes of a collision pair. - explicit ContactPatchRequest(size_t max_num_patch = 1, - size_t num_samples_curved_shapes = - ContactPatch::default_preallocated_size, - FCL_REAL patch_tolerance = 1e-3) - : max_num_patch(max_num_patch) { - this->setNumSamplesCurvedShapes(num_samples_curved_shapes); - this->setPatchTolerance(patch_tolerance); - } - - /// @brief Construct a contact patch request from a collision request. - explicit ContactPatchRequest(const CollisionRequest& collision_request, - size_t num_samples_curved_shapes = - ContactPatch::default_preallocated_size, - FCL_REAL patch_tolerance = 1e-3) - : max_num_patch(collision_request.num_max_contacts) { - this->setNumSamplesCurvedShapes(num_samples_curved_shapes); - this->setPatchTolerance(patch_tolerance); - } - - /// @copydoc m_num_samples_curved_shapes - void setNumSamplesCurvedShapes(const size_t num_samples_curved_shapes) { - if (num_samples_curved_shapes < 3) { - HPP_FCL_LOG_WARNING( - "`num_samples_curved_shapes` cannot be lower than 3. Setting it to " - "3 to prevent bugs."); - this->m_num_samples_curved_shapes = 3; - } else { - this->m_num_samples_curved_shapes = num_samples_curved_shapes; - } - } - - /// @copydoc m_num_samples_curved_shapes - size_t getNumSamplesCurvedShapes() const { - return this->m_num_samples_curved_shapes; - } - - /// @copydoc m_patch_tolerance - void setPatchTolerance(const FCL_REAL patch_tolerance) { - if (patch_tolerance < 0) { - HPP_FCL_LOG_WARNING( - "`patch_tolerance` cannot be negative. Setting it to 0 to prevent " - "bugs."); - this->m_patch_tolerance = Eigen::NumTraits::dummy_precision(); - } else { - this->m_patch_tolerance = patch_tolerance; - } - } - - /// @copydoc m_patch_tolerance - FCL_REAL getPatchTolerance() const { return this->m_patch_tolerance; } - - /// @brief Whether two ContactPatchRequest are identical or not. - bool operator==(const ContactPatchRequest& other) const { - return this->max_num_patch == other.max_num_patch && - this->getNumSamplesCurvedShapes() == - other.getNumSamplesCurvedShapes() && - this->getPatchTolerance() == other.getPatchTolerance(); - } -}; - -/// @brief Result for a contact patch computation. -struct HPP_FCL_DLLAPI ContactPatchResult { - using ContactPatchVector = std::vector; - using ContactPatchRef = std::reference_wrapper; - using ContactPatchRefVector = std::vector; - - protected: - /// @brief Data container for the vector of contact patches. - /// @note Contrary to `CollisionResult` or `DistanceResult`, which have a - /// very small memory footprint, contact patches can contain relatively - /// large polytopes. In order to reuse a `ContactPatchResult` while avoiding - /// successive mallocs, we have a data container and a vector which points - /// to the currently active patches in this data container. - ContactPatchVector m_contact_patches_data; - - /// @brief Contact patches in `m_contact_patches_data` can have two - /// statuses: used or unused. This index tracks the first unused patch in - /// the `m_contact_patches_data` vector. - size_t m_id_available_patch; - - /// @brief Vector of contact patches of the result. - ContactPatchRefVector m_contact_patches; - - public: - /// @brief Default constructor. - ContactPatchResult() : m_id_available_patch(0) { - const size_t max_num_patch = 1; - const ContactPatchRequest request(max_num_patch); - this->set(request); - } - - /// @brief Constructor using a `ContactPatchRequest`. - explicit ContactPatchResult(const ContactPatchRequest& request) - : m_id_available_patch(0) { - this->set(request); - }; - - /// @brief Number of contact patches in the result. - size_t numContactPatches() const { return this->m_contact_patches.size(); } - - /// @brief Returns a new unused contact patch from the internal data vector. - ContactPatchRef getUnusedContactPatch() { - if (this->m_id_available_patch >= this->m_contact_patches_data.size()) { - HPP_FCL_LOG_WARNING( - "Trying to get an unused contact patch but all contact patches are " - "used. Increasing size of contact patches vector, at the cost of a " - "copy. You should increase `max_num_patch` in the " - "`ContactPatchRequest`."); - this->m_contact_patches_data.emplace_back( - this->m_contact_patches_data.back()); - this->m_contact_patches_data.back().clear(); - } - ContactPatch& contact_patch = - this->m_contact_patches_data[this->m_id_available_patch]; - contact_patch.clear(); - this->m_contact_patches.emplace_back(contact_patch); - ++(this->m_id_available_patch); - return this->m_contact_patches.back(); - } - - /// @brief Const getter for the i-th contact patch of the result. - const ContactPatch& getContactPatch(const size_t i) const { - if (this->m_contact_patches.empty()) { - HPP_FCL_THROW_PRETTY( - "The number of contact patches is zero. No ContactPatch can be " - "returned.", - std::invalid_argument); - } - if (i < this->m_contact_patches.size()) { - return this->m_contact_patches[i]; - } - return this->m_contact_patches.back(); - } - - /// @brief Getter for the i-th contact patch of the result. - ContactPatch& contactPatch(const size_t i) { - if (this->m_contact_patches.empty()) { - HPP_FCL_THROW_PRETTY( - "The number of contact patches is zero. No ContactPatch can be " - "returned.", - std::invalid_argument); - } - if (i < this->m_contact_patches.size()) { - return this->m_contact_patches[i]; - } - return this->m_contact_patches.back(); - } - - /// @brief Clears the contact patch result. - void clear() { - this->m_contact_patches.clear(); - this->m_id_available_patch = 0; - for (ContactPatch& patch : this->m_contact_patches_data) { - patch.clear(); - } - } - - /// @brief Set up a `ContactPatchResult` from a `ContactPatchRequest` - void set(const ContactPatchRequest& request) { - if (this->m_contact_patches_data.size() < request.max_num_patch) { - this->m_contact_patches_data.resize(request.max_num_patch); - } - for (ContactPatch& patch : this->m_contact_patches_data) { - patch.points().reserve(request.getNumSamplesCurvedShapes()); - } - this->clear(); - } - - /// @brief Return true if this `ContactPatchResult` is aligned with the - /// `ContactPatchRequest` given as input. - bool check(const ContactPatchRequest& request) const { - assert(this->m_contact_patches_data.size() >= request.max_num_patch); - if (this->m_contact_patches_data.size() < request.max_num_patch) { - return false; - } - - for (const ContactPatch& patch : this->m_contact_patches_data) { - if (patch.points().capacity() < request.getNumSamplesCurvedShapes()) { - assert(patch.points().capacity() >= - request.getNumSamplesCurvedShapes()); - return false; - } - } - return true; - } - - /// @brief Whether two ContactPatchResult are identical or not. - bool operator==(const ContactPatchResult& other) const { - if (this->numContactPatches() != other.numContactPatches()) { - return false; - } - - for (size_t i = 0; i < this->numContactPatches(); ++i) { - const ContactPatch& patch = this->getContactPatch(i); - const ContactPatch& other_patch = other.getContactPatch(i); - if (!(patch == other_patch)) { - return false; - } - } - - return true; - } - - /// @brief Repositions the ContactPatch when they get inverted during their - /// construction. - void swapObjects() { - // Create new transform: it's the reflection of `tf` along the z-axis. - // This corresponds to doing a PI rotation along the y-axis, i.e. the x-axis - // becomes -x-axis, y-axis stays the same, z-axis becomes -z-axis. - for (size_t i = 0; i < this->numContactPatches(); ++i) { - ContactPatch& patch = this->contactPatch(i); - patch.tf.rotation().col(0) *= -1.0; - patch.tf.rotation().col(2) *= -1.0; - - for (size_t j = 0; j < patch.size(); ++j) { - patch.point(i)(0) *= -1.0; // only invert the x-axis - } - } - } -}; - -struct DistanceResult; - -/// @brief request to the distance computation -struct HPP_FCL_DLLAPI DistanceRequest : QueryRequest { - /// @brief whether to return the nearest points. - /// Nearest points are always computed and are the points of the shapes that - /// achieve a distance of `DistanceResult::min_distance`. - HPP_FCL_DEPRECATED_MESSAGE( - Nearest points are always computed : they are the points of the shapes - that achieve a distance of - `DistanceResult::min_distance` - .\n Use `enable_signed_distance` if you want to compute a signed - minimum distance(and thus its corresponding nearest points) - .) - bool enable_nearest_points; - - /// @brief whether to compute the penetration depth when objects are in - /// collision. - /// Turning this off can save computation time if only the distance - /// when objects are disjoint is needed. - /// @note The minimum distance between the shapes is stored in - /// `DistanceResult::min_distance`. - /// If `enable_signed_distance` is off, `DistanceResult::min_distance` - /// is always positive. - /// If `enable_signed_distance` is on, `DistanceResult::min_distance` - /// can be positive or negative. - /// The nearest points are the points of the shapes that achieve - /// a distance of `DistanceResult::min_distance`. - bool enable_signed_distance; - - /// @brief error threshold for approximate distance - FCL_REAL rel_err; // relative error, between 0 and 1 - FCL_REAL abs_err; // absolute error - - /// \param enable_nearest_points_ enables the nearest points computation. - /// \param enable_signed_distance_ allows to compute the penetration depth - /// \param rel_err_ - /// \param abs_err_ - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - DistanceRequest(bool enable_nearest_points_ = true, - bool enable_signed_distance_ = true, FCL_REAL rel_err_ = 0.0, - FCL_REAL abs_err_ = 0.0) - : enable_nearest_points(enable_nearest_points_), - enable_signed_distance(enable_signed_distance_), - rel_err(rel_err_), - abs_err(abs_err_) {} - HPP_FCL_COMPILER_DIAGNOSTIC_POP - - bool isSatisfied(const DistanceResult& result) const; - - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - DistanceRequest& operator=(const DistanceRequest& other) = default; - HPP_FCL_COMPILER_DIAGNOSTIC_POP - - /// @brief whether two DistanceRequest are the same or not - inline bool operator==(const DistanceRequest& other) const { - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - return QueryRequest::operator==(other) && - enable_nearest_points == other.enable_nearest_points && - enable_signed_distance == other.enable_signed_distance && - rel_err == other.rel_err && abs_err == other.abs_err; - HPP_FCL_COMPILER_DIAGNOSTIC_POP - } -}; - -/// @brief distance result -struct HPP_FCL_DLLAPI DistanceResult : QueryResult { - public: - /// @brief minimum distance between two objects. - /// If two objects are in collision and - /// DistanceRequest::enable_signed_distance is activated, min_distance <= 0. - /// @note The nearest points are the points of the shapes that achieve a - /// distance of `DistanceResult::min_distance`. - FCL_REAL min_distance; - - /// @brief normal. - Vec3f normal; - - /// @brief nearest points. - /// See CollisionResult::nearest_points. - std::array nearest_points; - - /// @brief collision object 1 - const CollisionGeometry* o1; - - /// @brief collision object 2 - const CollisionGeometry* o2; - - /// @brief information about the nearest point in object 1 - /// if object 1 is mesh or point cloud, it is the triangle or point id - /// if object 1 is geometry shape, it is NONE (-1), - /// if object 1 is octree, it is the id of the cell - int b1; - - /// @brief information about the nearest point in object 2 - /// if object 2 is mesh or point cloud, it is the triangle or point id - /// if object 2 is geometry shape, it is NONE (-1), - /// if object 2 is octree, it is the id of the cell - int b2; - - /// @brief invalid contact primitive information - static const int NONE = -1; - - DistanceResult( - FCL_REAL min_distance_ = (std::numeric_limits::max)()) - : min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) { - const Vec3f nan( - Vec3f::Constant(std::numeric_limits::quiet_NaN())); - nearest_points[0] = nearest_points[1] = normal = nan; - } - - /// @brief add distance information into the result - void update(FCL_REAL distance, const CollisionGeometry* o1_, - const CollisionGeometry* o2_, int b1_, int b2_) { - if (min_distance > distance) { - min_distance = distance; - o1 = o1_; - o2 = o2_; - b1 = b1_; - b2 = b2_; - } - } - - /// @brief add distance information into the result - void update(FCL_REAL distance, const CollisionGeometry* o1_, - const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1, - const Vec3f& p2, const Vec3f& normal_) { - if (min_distance > distance) { - min_distance = distance; - o1 = o1_; - o2 = o2_; - b1 = b1_; - b2 = b2_; - nearest_points[0] = p1; - nearest_points[1] = p2; - normal = normal_; - } - } - - /// @brief add distance information into the result - void update(const DistanceResult& other_result) { - if (min_distance > other_result.min_distance) { - min_distance = other_result.min_distance; - o1 = other_result.o1; - o2 = other_result.o2; - b1 = other_result.b1; - b2 = other_result.b2; - nearest_points[0] = other_result.nearest_points[0]; - nearest_points[1] = other_result.nearest_points[1]; - normal = other_result.normal; - } - } - - /// @brief clear the result - void clear() { - const Vec3f nan( - Vec3f::Constant(std::numeric_limits::quiet_NaN())); - min_distance = (std::numeric_limits::max)(); - o1 = NULL; - o2 = NULL; - b1 = NONE; - b2 = NONE; - nearest_points[0] = nearest_points[1] = normal = nan; - timings.clear(); - } - - /// @brief whether two DistanceResult are the same or not - inline bool operator==(const DistanceResult& other) const { - bool is_same = min_distance == other.min_distance && - nearest_points[0] == other.nearest_points[0] && - nearest_points[1] == other.nearest_points[1] && - normal == other.normal && o1 == other.o1 && o2 == other.o2 && - b1 == other.b1 && b2 == other.b2; - - // TODO: check also that two GeometryObject are indeed equal. - if ((o1 != NULL) ^ (other.o1 != NULL)) return false; - is_same &= (o1 == other.o1); - // else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 == - // *other.o1; - - if ((o2 != NULL) ^ (other.o2 != NULL)) return false; - is_same &= (o2 == other.o2); - // else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 == - // *other.o2; - - return is_same; - } -}; - -namespace internal { -inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/, - CollisionResult& res, - const FCL_REAL sqrDistLowerBound) { - // BV cannot find negative distance. - if (res.distance_lower_bound <= 0) return; - FCL_REAL new_dlb = std::sqrt(sqrDistLowerBound); // - req.security_margin; - if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb; -} - -inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&, - CollisionResult& res, - const FCL_REAL& distance, - const Vec3f& p0, const Vec3f& p1, - const Vec3f& normal) { - if (distance < res.distance_lower_bound) { - res.distance_lower_bound = distance; - res.nearest_points[0] = p0; - res.nearest_points[1] = p1; - res.normal = normal; - } -} -} // namespace internal - -inline CollisionRequestFlag operator~(CollisionRequestFlag a) { - return static_cast(~static_cast(a)); -} - -inline CollisionRequestFlag operator|(CollisionRequestFlag a, - CollisionRequestFlag b) { - return static_cast(static_cast(a) | - static_cast(b)); -} - -inline CollisionRequestFlag operator&(CollisionRequestFlag a, - CollisionRequestFlag b) { - return static_cast(static_cast(a) & - static_cast(b)); -} - -inline CollisionRequestFlag operator^(CollisionRequestFlag a, - CollisionRequestFlag b) { - return static_cast(static_cast(a) ^ - static_cast(b)); -} - -inline CollisionRequestFlag& operator|=(CollisionRequestFlag& a, - CollisionRequestFlag b) { - return (CollisionRequestFlag&)((int&)(a) |= static_cast(b)); -} - -inline CollisionRequestFlag& operator&=(CollisionRequestFlag& a, - CollisionRequestFlag b) { - return (CollisionRequestFlag&)((int&)(a) &= static_cast(b)); -} - -inline CollisionRequestFlag& operator^=(CollisionRequestFlag& a, - CollisionRequestFlag b) { - return (CollisionRequestFlag&)((int&)(a) ^= static_cast(b)); -} - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/collision_func_matrix.h b/include/hpp/fcl/collision_func_matrix.h index 57b78bdfa..c17b82412 100644 --- a/include/hpp/fcl/collision_func_matrix.h +++ b/include/hpp/fcl/collision_func_matrix.h @@ -1,80 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_COLLISION_FUNC_MATRIX_H -#define HPP_FCL_COLLISION_FUNC_MATRIX_H - -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// @brief collision matrix stores the functions for collision between different -/// types of objects and provides a uniform call interface - -struct HPP_FCL_DLLAPI CollisionFunctionMatrix { - /// @brief the uniform call interface for collision: for collision, we need - /// know - /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 - /// and tf2; - /// 2. the solver for narrow phase collision, this is for the collision - /// between geometric shapes; - /// 3. the request setting for collision (e.g., whether need to return normal - /// information, whether need to compute cost); - /// 4. the structure to return collision result - typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result); - - /// @brief each item in the collision matrix is a function to handle collision - /// between objects of type1 and type2 - CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT]; - - CollisionFunctionMatrix(); -}; - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/collision_object.h b/include/hpp/fcl/collision_object.h index fae651464..245da1c07 100644 --- a/include/hpp/fcl/collision_object.h +++ b/include/hpp/fcl/collision_object.h @@ -1,363 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_COLLISION_OBJECT_BASE_H -#define HPP_FCL_COLLISION_OBJECT_BASE_H - -#include -#include - -#include -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// @brief object type: BVH (mesh, points), basic geometry, octree -enum OBJECT_TYPE { - OT_UNKNOWN, - OT_BVH, - OT_GEOM, - OT_OCTREE, - OT_HFIELD, - OT_COUNT -}; - -/// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, -/// KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, -/// cylinder, convex, plane, triangle), and octree -enum NODE_TYPE { - BV_UNKNOWN, - BV_AABB, - BV_OBB, - BV_RSS, - BV_kIOS, - BV_OBBRSS, - BV_KDOP16, - BV_KDOP18, - BV_KDOP24, - GEOM_BOX, - GEOM_SPHERE, - GEOM_CAPSULE, - GEOM_CONE, - GEOM_CYLINDER, - GEOM_CONVEX, - GEOM_PLANE, - GEOM_HALFSPACE, - GEOM_TRIANGLE, - GEOM_OCTREE, - GEOM_ELLIPSOID, - HF_AABB, - HF_OBBRSS, - NODE_COUNT -}; - -/// @addtogroup Construction_Of_BVH -/// @{ - -/// @brief The geometry for the object for collision or distance computation -class HPP_FCL_DLLAPI CollisionGeometry { - public: - CollisionGeometry() - : aabb_center(Vec3f::Constant((std::numeric_limits::max)())), - aabb_radius(-1), - cost_density(1), - threshold_occupied(1), - threshold_free(0) {} - - /// \brief Copy constructor - CollisionGeometry(const CollisionGeometry& other) = default; - - virtual ~CollisionGeometry() {} - - /// @brief Clone *this into a new CollisionGeometry - virtual CollisionGeometry* clone() const = 0; - - /// \brief Equality operator - bool operator==(const CollisionGeometry& other) const { - return cost_density == other.cost_density && - threshold_occupied == other.threshold_occupied && - threshold_free == other.threshold_free && - aabb_center == other.aabb_center && - aabb_radius == other.aabb_radius && aabb_local == other.aabb_local && - isEqual(other); - } - - /// \brief Difference operator - bool operator!=(const CollisionGeometry& other) const { - return isNotEqual(other); - } - - /// @brief get the type of the object - virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; } - - /// @brief get the node type - virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; } - - /// @brief compute the AABB for object in local coordinate - virtual void computeLocalAABB() = 0; - - /// @brief get user data in geometry - void* getUserData() const { return user_data; } - - /// @brief set user data in geometry - void setUserData(void* data) { user_data = data; } - - /// @brief whether the object is completely occupied - inline bool isOccupied() const { return cost_density >= threshold_occupied; } - - /// @brief whether the object is completely free - inline bool isFree() const { return cost_density <= threshold_free; } - - /// @brief whether the object has some uncertainty - bool isUncertain() const; - - /// @brief AABB center in local coordinate - Vec3f aabb_center; - - /// @brief AABB radius - FCL_REAL aabb_radius; - - /// @brief AABB in local coordinate, used for tight AABB when only translation - /// transform - AABB aabb_local; - - /// @brief pointer to user defined data specific to this object - void* user_data; - - /// @brief collision cost for unit volume - FCL_REAL cost_density; - - /// @brief threshold for occupied ( >= is occupied) - FCL_REAL threshold_occupied; - - /// @brief threshold for free (<= is free) - FCL_REAL threshold_free; - - /// @brief compute center of mass - virtual Vec3f computeCOM() const { return Vec3f::Zero(); } - - /// @brief compute the inertia matrix, related to the origin - virtual Matrix3f computeMomentofInertia() const { - return Matrix3f::Constant(NAN); - } - - /// @brief compute the volume - virtual FCL_REAL computeVolume() const { return 0; } - - /// @brief compute the inertia matrix, related to the com - virtual Matrix3f computeMomentofInertiaRelatedToCOM() const { - Matrix3f C = computeMomentofInertia(); - Vec3f com = computeCOM(); - FCL_REAL V = computeVolume(); - - return (Matrix3f() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]), - C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2], - C(1, 0) + V * com[1] * com[0], - C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]), - C(1, 2) + V * com[1] * com[2], C(2, 0) + V * com[2] * com[0], - C(2, 1) + V * com[2] * com[1], - C(2, 2) - V * (com[0] * com[0] + com[1] * com[1])) - .finished(); - } - - private: - /// @brief equal operator with another object of derived type. - virtual bool isEqual(const CollisionGeometry& other) const = 0; - - /// @brief not equal operator with another object of derived type. - virtual bool isNotEqual(const CollisionGeometry& other) const { - return !(*this == other); - } - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/// @brief the object for collision or distance computation, contains the -/// geometry and the transform information -class HPP_FCL_DLLAPI CollisionObject { - public: - CollisionObject(const shared_ptr& cgeom_, - bool compute_local_aabb = true) - : cgeom(cgeom_), user_data(nullptr) { - init(compute_local_aabb); - } - - CollisionObject(const shared_ptr& cgeom_, - const Transform3f& tf, bool compute_local_aabb = true) - : cgeom(cgeom_), t(tf), user_data(nullptr) { - init(compute_local_aabb); - } - - CollisionObject(const shared_ptr& cgeom_, - const Matrix3f& R, const Vec3f& T, - bool compute_local_aabb = true) - : cgeom(cgeom_), t(R, T), user_data(nullptr) { - init(compute_local_aabb); - } - - bool operator==(const CollisionObject& other) const { - return cgeom == other.cgeom && t == other.t && user_data == other.user_data; - } - - bool operator!=(const CollisionObject& other) const { - return !(*this == other); - } - - ~CollisionObject() {} - - /// @brief get the type of the object - OBJECT_TYPE getObjectType() const { return cgeom->getObjectType(); } - - /// @brief get the node type - NODE_TYPE getNodeType() const { return cgeom->getNodeType(); } - - /// @brief get the AABB in world space - const AABB& getAABB() const { return aabb; } - - /// @brief get the AABB in world space - AABB& getAABB() { return aabb; } - - /// @brief compute the AABB in world space - void computeAABB() { - if (t.getRotation().isIdentity()) { - aabb = translate(cgeom->aabb_local, t.getTranslation()); - } else { - aabb.min_ = aabb.max_ = t.getTranslation(); - - Vec3f min_world, max_world; - for (int k = 0; k < 3; ++k) { - min_world.array() = t.getRotation().row(k).array() * - cgeom->aabb_local.min_.transpose().array(); - max_world.array() = t.getRotation().row(k).array() * - cgeom->aabb_local.max_.transpose().array(); - - aabb.min_[k] += (min_world.array().min)(max_world.array()).sum(); - aabb.max_[k] += (min_world.array().max)(max_world.array()).sum(); - } - } - } - - /// @brief get user data in object - void* getUserData() const { return user_data; } - - /// @brief set user data in object - void setUserData(void* data) { user_data = data; } - - /// @brief get translation of the object - inline const Vec3f& getTranslation() const { return t.getTranslation(); } - - /// @brief get matrix rotation of the object - inline const Matrix3f& getRotation() const { return t.getRotation(); } - - /// @brief get object's transform - inline const Transform3f& getTransform() const { return t; } - - /// @brief set object's rotation matrix - void setRotation(const Matrix3f& R) { t.setRotation(R); } - - /// @brief set object's translation - void setTranslation(const Vec3f& T) { t.setTranslation(T); } - - /// @brief set object's transform - void setTransform(const Matrix3f& R, const Vec3f& T) { t.setTransform(R, T); } - - /// @brief set object's transform - void setTransform(const Transform3f& tf) { t = tf; } - - /// @brief whether the object is in local coordinate - bool isIdentityTransform() const { return t.isIdentity(); } - - /// @brief set the object in local coordinate - void setIdentityTransform() { t.setIdentity(); } - - /// @brief get shared pointer to collision geometry of the object instance - const shared_ptr collisionGeometry() const { - return cgeom; - } - - /// @brief get shared pointer to collision geometry of the object instance - const shared_ptr& collisionGeometry() { return cgeom; } - - /// @brief get raw pointer to collision geometry of the object instance - const CollisionGeometry* collisionGeometryPtr() const { return cgeom.get(); } - - /// @brief get raw pointer to collision geometry of the object instance - CollisionGeometry* collisionGeometryPtr() { return cgeom.get(); } - - /// @brief Associate a new CollisionGeometry - /// - /// @param[in] collision_geometry The new CollisionGeometry - /// @param[in] compute_local_aabb Whether the local aabb of the input new has - /// to be computed. - /// - void setCollisionGeometry( - const shared_ptr& collision_geometry, - bool compute_local_aabb = true) { - if (collision_geometry.get() != cgeom.get()) { - cgeom = collision_geometry; - init(compute_local_aabb); - } - } - - protected: - void init(bool compute_local_aabb = true) { - if (cgeom) { - if (compute_local_aabb) cgeom->computeLocalAABB(); - computeAABB(); - } - } - - shared_ptr cgeom; - - Transform3f t; - - /// @brief AABB in global coordinate - mutable AABB aabb; - - /// @brief pointer to user defined data specific to this object - void* user_data; -}; - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/collision_utility.h b/include/hpp/fcl/collision_utility.h index b231973ec..9043e1f8e 100644 --- a/include/hpp/fcl/collision_utility.h +++ b/include/hpp/fcl/collision_utility.h @@ -1,59 +1,2 @@ -// Copyright (c) 2017 CNRS -// Copyright (c) 2022 INRIA -// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) -// -// This file is part of hpp-fcl. -// hpp-fcl is free software: you can redistribute it -// and/or modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation, either version -// 3 of the License, or (at your option) any later version. -// -// hpp-fcl is distributed in the hope that it will be -// useful, but WITHOUT ANY WARRANTY; without even the implied warranty -// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -// General Lesser Public License for more details. You should have -// received a copy of the GNU Lesser General Public License along with -// hpp-fcl. If not, see . - -#ifndef HPP_FCL_COLLISION_UTILITY_H -#define HPP_FCL_COLLISION_UTILITY_H - -#include - -namespace hpp { -namespace fcl { - -HPP_FCL_DLLAPI CollisionGeometry* extract(const CollisionGeometry* model, - const Transform3f& pose, - const AABB& aabb); - -/** - * \brief Returns the name associated to a NODE_TYPE - */ -inline const char* get_node_type_name(NODE_TYPE node_type) { - static const char* node_type_name_all[] = { - "BV_UNKNOWN", "BV_AABB", "BV_OBB", "BV_RSS", - "BV_kIOS", "BV_OBBRSS", "BV_KDOP16", "BV_KDOP18", - "BV_KDOP24", "GEOM_BOX", "GEOM_SPHERE", "GEOM_CAPSULE", - "GEOM_CONE", "GEOM_CYLINDER", "GEOM_CONVEX", "GEOM_PLANE", - "GEOM_HALFSPACE", "GEOM_TRIANGLE", "GEOM_OCTREE", "GEOM_ELLIPSOID", - "HF_AABB", "HF_OBBRSS", "NODE_COUNT"}; - - return node_type_name_all[node_type]; -} - -/** - * \brief Returns the name associated to a OBJECT_TYPE - */ -inline const char* get_object_type_name(OBJECT_TYPE object_type) { - static const char* object_type_name_all[] = { - "OT_UNKNOWN", "OT_BVH", "OT_GEOM", "OT_OCTREE", "OT_HFIELD", "OT_COUNT"}; - - return object_type_name_all[object_type]; -} - -} // namespace fcl - -} // namespace hpp - -#endif // FCL_COLLISION_UTILITY_H +#include +#include diff --git a/include/hpp/fcl/config.hh b/include/hpp/fcl/config.hh new file mode 100644 index 000000000..7388d6087 --- /dev/null +++ b/include/hpp/fcl/config.hh @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/contact_patch.h b/include/hpp/fcl/contact_patch.h index e3dfa7547..fef1ee0d4 100644 --- a/include/hpp/fcl/contact_patch.h +++ b/include/hpp/fcl/contact_patch.h @@ -1,124 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2024, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of INRIA nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Louis Montaut */ - -#ifndef HPP_FCL_CONTACT_PATCH_H -#define HPP_FCL_CONTACT_PATCH_H - -#include "hpp/fcl/data_types.h" -#include "hpp/fcl/collision_data.h" -#include "hpp/fcl/contact_patch/contact_patch_solver.h" -#include "hpp/fcl/contact_patch_func_matrix.h" - -namespace hpp { -namespace fcl { - -/// @brief Main contact patch computation interface. -/// @note Please see @ref ContactPatchRequest and @ref ContactPatchResult for -/// more info on the content of the input/output of this function. Also, please -/// read @ref ContactPatch if you want to fully understand what is meant by -/// "contact patch". -HPP_FCL_DLLAPI void computeContactPatch(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const CollisionResult& collision_result, - const ContactPatchRequest& request, - ContactPatchResult& result); - -/// @copydoc computeContactPatch(const CollisionGeometry*, const Transform3f&, -// const CollisionGeometry*, const Transform3f&, const CollisionResult&, const -// ContactPatchRequest&, ContactPatchResult&); -HPP_FCL_DLLAPI void computeContactPatch(const CollisionObject* o1, - const CollisionObject* o2, - const CollisionResult& collision_result, - const ContactPatchRequest& request, - ContactPatchResult& result); - -/// @brief This class reduces the cost of identifying the geometry pair. -/// This is usefull for repeated shape-shape queries. -/// @note This needs to be called after `collide` or after `ComputeCollision`. -/// -/// \code -/// ComputeContactPatch calc_patch (o1, o2); -/// calc_patch(tf1, tf2, collision_result, patch_request, patch_result); -/// \endcode -class HPP_FCL_DLLAPI ComputeContactPatch { - public: - /// @brief Default constructor from two Collision Geometries. - ComputeContactPatch(const CollisionGeometry* o1, const CollisionGeometry* o2); - - void operator()(const Transform3f& tf1, const Transform3f& tf2, - const CollisionResult& collision_result, - const ContactPatchRequest& request, - ContactPatchResult& result) const; - - bool operator==(const ComputeContactPatch& other) const { - return this->o1 == other.o1 && this->o2 == other.o2 && - this->csolver == other.csolver; - } - - bool operator!=(const ComputeContactPatch& other) const { - return !(*this == other); - } - - virtual ~ComputeContactPatch() = default; - - protected: - // These pointers are made mutable to let the derived classes to update - // their values when updating the collision geometry (e.g. creating a new - // one). This feature should be used carefully to avoid any mis usage (e.g, - // changing the type of the collision geometry should be avoided). - mutable const CollisionGeometry* o1; - mutable const CollisionGeometry* o2; - - mutable ContactPatchSolver csolver; - - ContactPatchFunctionMatrix::ContactPatchFunc func; - bool swap_geoms; - - virtual void run(const Transform3f& tf1, const Transform3f& tf2, - const CollisionResult& collision_result, - const ContactPatchRequest& request, - ContactPatchResult& result) const; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/contact_patch/contact_patch_solver.h b/include/hpp/fcl/contact_patch/contact_patch_solver.h index ed2ae9478..f53a91507 100644 --- a/include/hpp/fcl/contact_patch/contact_patch_solver.h +++ b/include/hpp/fcl/contact_patch/contact_patch_solver.h @@ -1,210 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2024, INRIA - * All rights reserved. - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of INRIA nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Louis Montaut */ - -#ifndef HPP_FCL_CONTACT_PATCH_SOLVER_H -#define HPP_FCL_CONTACT_PATCH_SOLVER_H - -#include "hpp/fcl/collision_data.h" -#include "hpp/fcl/logging.h" -#include "hpp/fcl/narrowphase/gjk.h" - -namespace hpp { -namespace fcl { - -/// @brief Solver to compute contact patches, i.e. the intersection between two -/// contact surfaces projected onto the shapes' separating plane. -/// Otherwise said, a contact patch is simply the intersection between two -/// support sets: the support set of shape S1 in direction `n` and the support -/// set of shape S2 in direction `-n`, where `n` is the contact normal -/// (satisfying the optimality conditions of GJK/EPA). -/// @note A contact patch is **not** the support set of the Minkowski Difference -/// in the direction of the normal. -/// A contact patch is actually the support set of the Minkowski difference in -/// the direction of the normal, i.e. the instersection of the shapes support -/// sets as mentioned above. -/// -/// TODO(louis): algo improvement: -/// - The clipping algo is currently n1 * n2; it can be done in n1 + n2. -struct HPP_FCL_DLLAPI ContactPatchSolver { - // Note: `ContactPatch` is an alias for `SupportSet`. - // The two can be used interchangeably. - using ShapeSupportData = details::ShapeSupportData; - using SupportSetDirection = SupportSet::PatchDirection; - - /// @brief Support set function for shape si. - /// @param[in] shape the shape. - /// @param[in/out] support_set a support set of the shape. A support set is - /// attached to a frame. All the points of the set computed by this function - /// will be expressed in the local frame of the support set. The support set - /// is computed in the direction of the positive z-axis if its direction is - /// DEFAULT, negative z-axis if its direction is INVERTED. - /// @param[in/out] hint for the support computation of ConvexBase shapes. Gets - /// updated after calling the function onto ConvexBase shapes. - /// @param[in/out] support_data for the support computation of ConvexBase - /// shapes. Gets updated with visited vertices after calling the function onto - /// ConvexBase shapes. - /// @param[in] num_sampled_supports for shapes like cone or cylinders which - /// have smooth non-strictly convex sides (their bases are circles), we need - /// to know how many supports we sample from these sides. For any other shape, - /// this parameter is not used. - /// @param[in] tol the "thickness" of the support plane. Any point v which - /// satisfies `max_{x in shape}(x.dot(dir)) - v.dot(dir) <= tol` is tol - /// distant from the support plane and is added to the support set. - typedef void (*SupportSetFunction)(const ShapeBase* shape, - SupportSet& support_set, int& hint, - ShapeSupportData& support_data, - size_t num_sampled_supports, FCL_REAL tol); - - /// @brief Number of vectors to pre-allocate in the `m_clipping_sets` vectors. - static constexpr size_t default_num_preallocated_supports = 16; - - /// @brief Number of points sampled for Cone and Cylinder when the normal is - /// orthogonal to the shapes' basis. - /// See @ref ContactPatchRequest::m_num_samples_curved_shapes for more - /// details. - size_t num_samples_curved_shapes; - - /// @brief Tolerance below which points are added to the shapes support sets. - /// See @ref ContactPatchRequest::m_patch_tolerance for more details. - FCL_REAL patch_tolerance; - - /// @brief Support set function for shape s1. - mutable SupportSetFunction supportFuncShape1; - - /// @brief Support set function for shape s2. - mutable SupportSetFunction supportFuncShape2; - - /// @brief Temporary data to compute the support sets on each shape. - mutable std::array supports_data; - - /// @brief Guess for the support sets computation. - mutable support_func_guess_t support_guess; - - /// @brief Holder for support set of shape 1, used for internal computation. - /// After `computePatch` has been called, this support set is no longer valid. - mutable SupportSet support_set_shape1; - - /// @brief Holder for support set of shape 2, used for internal computation. - /// After `computePatch` has been called, this support set is no longer valid. - mutable SupportSet support_set_shape2; - - /// @brief Temporary support set used for the Sutherland-Hodgman algorithm. - mutable SupportSet support_set_buffer; - - /// @brief Tracks which point of the Sutherland-Hodgman result have been added - /// to the contact patch. Only used if the post-processing step occurs, i.e. - /// if the result of Sutherland-Hodgman has a size bigger than - /// `max_patch_size`. - mutable std::vector added_to_patch; - - /// @brief Default constructor. - explicit ContactPatchSolver() { - const size_t num_contact_patch = 1; - const size_t preallocated_patch_size = - ContactPatch::default_preallocated_size; - const FCL_REAL patch_tolerance = 1e-3; - const ContactPatchRequest request(num_contact_patch, - preallocated_patch_size, patch_tolerance); - this->set(request); - } - - /// @brief Construct the solver with a `ContactPatchRequest`. - explicit ContactPatchSolver(const ContactPatchRequest& request) { - this->set(request); - } - - /// @brief Set up the solver using a `ContactPatchRequest`. - void set(const ContactPatchRequest& request); - - /// @brief Sets the support guess used during support set computation of - /// shapes s1 and s2. - void setSupportGuess(const support_func_guess_t guess) const { - this->support_guess = guess; - } - - /// @brief Main API of the solver: compute a contact patch from a contact - /// between shapes s1 and s2. - /// The contact patch is the (triple) intersection between the separating - /// plane passing (by `contact.pos` and supported by `contact.normal`) and the - /// shapes s1 and s2. - template - void computePatch(const ShapeType1& s1, const Transform3f& tf1, - const ShapeType2& s2, const Transform3f& tf2, - const Contact& contact, ContactPatch& contact_patch) const; - - /// @brief Reset the internal quantities of the solver. - template - void reset(const ShapeType1& shape1, const Transform3f& tf1, - const ShapeType2& shape2, const Transform3f& tf2, - const ContactPatch& contact_patch) const; - - /// @brief Retrieve result, adds a post-processing step if result has bigger - /// size than `this->max_patch_size`. - void getResult(const Contact& contact, const ContactPatch::Polygon* result, - ContactPatch& contact_patch) const; - - /// @return the intersecting point between line defined by ray (a, b) and - /// the segment [c, d]. - /// @note we make the following hypothesis: - /// 1) c != d (should be when creating initial polytopes) - /// 2) (c, d) is not parallel to ray -> if so, we return d. - static Vec2f computeLineSegmentIntersection(const Vec2f& a, const Vec2f& b, - const Vec2f& c, const Vec2f& d); - - /// @brief Construct support set function for shape. - static SupportSetFunction makeSupportSetFunction( - const ShapeBase* shape, ShapeSupportData& support_data); - - bool operator==(const ContactPatchSolver& other) const { - return this->num_samples_curved_shapes == other.num_samples_curved_shapes && - this->patch_tolerance == other.patch_tolerance && - this->support_guess == other.support_guess && - this->support_set_shape1 == other.support_set_shape1 && - this->support_set_shape2 == other.support_set_shape2 && - this->support_set_buffer == other.support_set_buffer && - this->added_to_patch == other.added_to_patch && - this->supportFuncShape1 == other.supportFuncShape1 && - this->supportFuncShape2 == other.supportFuncShape2; - } - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -} // namespace fcl -} // namespace hpp - -#include "hpp/fcl/contact_patch/contact_patch_solver.hxx" - -#endif // HPP_FCL_CONTACT_PATCH_SOLVER_H +#include +#include diff --git a/include/hpp/fcl/contact_patch/contact_patch_solver.hxx b/include/hpp/fcl/contact_patch/contact_patch_solver.hxx index 1960ff0ac..b73d4e935 100644 --- a/include/hpp/fcl/contact_patch/contact_patch_solver.hxx +++ b/include/hpp/fcl/contact_patch/contact_patch_solver.hxx @@ -1,427 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2024, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of INRIA nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Louis Montaut */ - -#ifndef HPP_FCL_CONTACT_PATCH_SOLVER_HXX -#define HPP_FCL_CONTACT_PATCH_SOLVER_HXX - -#include "hpp/fcl/data_types.h" -#include "hpp/fcl/shape/geometric_shapes_traits.h" - -namespace hpp { -namespace fcl { - -// ============================================================================ -inline void ContactPatchSolver::set(const ContactPatchRequest& request) { - // Note: it's important for the number of pre-allocated Vec3f in - // `m_clipping_sets` to be larger than `request.max_size_patch` - // because we don't know in advance how many supports will be discarded to - // form the convex-hulls of the shapes supports which will serve as the - // input of the Sutherland-Hodgman algorithm. - size_t num_preallocated_supports = default_num_preallocated_supports; - if (num_preallocated_supports < 2 * request.getNumSamplesCurvedShapes()) { - num_preallocated_supports = 2 * request.getNumSamplesCurvedShapes(); - } - - // Used for support set computation of shape1 and for the first iterate of the - // Sutherland-Hodgman algo. - this->support_set_shape1.points().reserve(num_preallocated_supports); - this->support_set_shape1.direction = SupportSetDirection::DEFAULT; - - // Used for computing the next iterate of the Sutherland-Hodgman algo. - this->support_set_buffer.points().reserve(num_preallocated_supports); - - // Used for support set computation of shape2 and acts as the "clipper" set in - // the Sutherland-Hodgman algo. - this->support_set_shape2.points().reserve(num_preallocated_supports); - this->support_set_shape2.direction = SupportSetDirection::INVERTED; - - this->num_samples_curved_shapes = request.getNumSamplesCurvedShapes(); - this->patch_tolerance = request.getPatchTolerance(); -} - -// ============================================================================ -template -void ContactPatchSolver::computePatch(const ShapeType1& s1, - const Transform3f& tf1, - const ShapeType2& s2, - const Transform3f& tf2, - const Contact& contact, - ContactPatch& contact_patch) const { - // Note: `ContactPatch` is an alias for `SupportSet`. - // Step 1 - constructContactPatchFrameFromContact(contact, contact_patch); - contact_patch.points().clear(); - if ((bool)(shape_traits::IsStrictlyConvex) || - (bool)(shape_traits::IsStrictlyConvex)) { - // If a shape is strictly convex, the support set in any direction is - // reduced to a single point. Thus, the contact point `contact.pos` is the - // only point belonging to the contact patch, and it has already been - // computed. - // TODO(louis): even for strictly convex shapes, we can sample the support - // function around the normal and return a pseudo support set. This would - // allow spheres and ellipsoids to have a contact surface, which does make - // sense in certain physics simulation cases. - // Do the same for strictly convex regions of non-strictly convex shapes - // like the ends of capsules. - contact_patch.addPoint(contact.pos); - return; - } - - // Step 2 - Compute support set of each shape, in the direction of - // the contact's normal. - // The first shape's support set is called "current"; it will be the first - // iterate of the Sutherland-Hodgman algorithm. The second shape's support set - // is called "clipper"; it will be used to clip "current". The support set - // computation step computes a convex polygon; its vertices are ordered - // counter-clockwise. This is important as the Sutherland-Hodgman algorithm - // expects points to be ranked counter-clockwise. - this->reset(s1, tf1, s2, tf2, contact_patch); - assert(this->num_samples_curved_shapes > 3); - - this->supportFuncShape1(&s1, this->support_set_shape1, this->support_guess[0], - this->supports_data[0], - this->num_samples_curved_shapes, - this->patch_tolerance); - - this->supportFuncShape2(&s2, this->support_set_shape2, this->support_guess[1], - this->supports_data[1], - this->num_samples_curved_shapes, - this->patch_tolerance); - - // We can immediatly return if one of the support set has only - // one point. - if (this->support_set_shape1.size() <= 1 || - this->support_set_shape2.size() <= 1) { - contact_patch.addPoint(contact.pos); - return; - } - - // `eps` is be used to check strict positivity of determinants. - const FCL_REAL eps = Eigen::NumTraits::dummy_precision(); - using Polygon = SupportSet::Polygon; - - if ((this->support_set_shape1.size() == 2) && - (this->support_set_shape2.size() == 2)) { - // Segment-Segment case - // We compute the determinant; if it is non-zero, the intersection - // has already been computed: it's `Contact::pos`. - const Polygon& pts1 = this->support_set_shape1.points(); - const Vec2f& a = pts1[0]; - const Vec2f& b = pts1[1]; - - const Polygon& pts2 = this->support_set_shape2.points(); - const Vec2f& c = pts2[0]; - const Vec2f& d = pts2[1]; - - const FCL_REAL det = - (b(0) - a(0)) * (d(1) - c(1)) >= (b(1) - a(1)) * (d(0) - c(0)); - if ((std::abs(det) > eps) || ((c - d).squaredNorm() < eps) || - ((b - a).squaredNorm() < eps)) { - contact_patch.addPoint(contact.pos); - return; - } - - const Vec2f cd = (d - c); - const FCL_REAL l = cd.squaredNorm(); - Polygon& patch = contact_patch.points(); - - // Project a onto [c, d] - FCL_REAL t1 = (a - c).dot(cd); - t1 = (t1 >= l) ? 1.0 : ((t1 <= 0) ? 0.0 : (t1 / l)); - const Vec2f p1 = c + t1 * cd; - patch.emplace_back(p1); - - // Project b onto [c, d] - FCL_REAL t2 = (b - c).dot(cd); - t2 = (t2 >= l) ? 1.0 : ((t2 <= 0) ? 0.0 : (t2 / l)); - const Vec2f p2 = c + t2 * cd; - if ((p1 - p2).squaredNorm() >= eps) { - patch.emplace_back(p2); - } - return; - } - - // - // Step 3 - Main loop of the algorithm: use the "clipper" polygon to clip the - // "current" polygon. The resulting intersection is the contact patch of the - // contact between s1 and s2. "clipper" and "current" are the support sets of - // shape1 and shape2 (they can be swapped, i.e. clipper can be assigned to - // shape1 and current to shape2, depending on which case we are). Currently, - // to clip one polygon with the other, we use the Sutherland-Hodgman - // algorithm: - // https://en.wikipedia.org/wiki/Sutherland%E2%80%93Hodgman_algorithm - // In the general case, Sutherland-Hodgman clips one polygon of size >=3 using - // another polygon of size >=3. However, it can be easily extended to handle - // the segment-polygon case. - // - // The maximum size of the output of the Sutherland-Hodgman algorithm is n1 + - // n2 where n1 and n2 are the sizes of the first and second polygon. - const size_t max_result_size = - this->support_set_shape1.size() + this->support_set_shape2.size(); - if (this->added_to_patch.size() < max_result_size) { - this->added_to_patch.assign(max_result_size, false); - } - - const Polygon* clipper_ptr = nullptr; - Polygon* current_ptr = nullptr; - Polygon* previous_ptr = &(this->support_set_buffer.points()); - - // Let the clipper set be the one with the most vertices, to make sure it is - // at least a triangle. - if (this->support_set_shape1.size() < this->support_set_shape2.size()) { - current_ptr = &(this->support_set_shape1.points()); - clipper_ptr = &(this->support_set_shape2.points()); - } else { - current_ptr = &(this->support_set_shape2.points()); - clipper_ptr = &(this->support_set_shape1.points()); - } - - const Polygon& clipper = *(clipper_ptr); - const size_t clipper_size = clipper.size(); - for (size_t i = 0; i < clipper_size; ++i) { - // Swap `current` and `previous`. - // `previous` tracks the last iteration of the algorithm; `current` is - // filled by clipping `current` using `clipper`. - Polygon* tmp_ptr = previous_ptr; - previous_ptr = current_ptr; - current_ptr = tmp_ptr; - - const Polygon& previous = *(previous_ptr); - Polygon& current = *(current_ptr); - current.clear(); - - const Vec2f& a = clipper[i]; - const Vec2f& b = clipper[(i + 1) % clipper_size]; - const Vec2f ab = b - a; - - if (previous.size() == 2) { - // - // Segment-Polygon case - // - const Vec2f& p1 = previous[0]; - const Vec2f& p2 = previous[1]; - - const Vec2f ap1 = p1 - a; - const Vec2f ap2 = p2 - a; - - const FCL_REAL det1 = ab(0) * ap1(1) - ab(1) * ap1(0); - const FCL_REAL det2 = ab(0) * ap2(1) - ab(1) * ap2(0); - - if (det1 < 0 && det2 < 0) { - // Both p1 and p2 are outside the clipping polygon, i.e. there is no - // intersection. The algorithm can stop. - break; - } - - if (det1 >= 0 && det2 >= 0) { - // Both p1 and p2 are inside the clipping polygon, there is nothing to - // do; move to the next iteration. - current = previous; - continue; - } - - // Compute the intersection between the line (a, b) and the segment - // [p1, p2]. - if (det1 >= 0) { - if (det1 > eps) { - const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2); - current.emplace_back(p1); - current.emplace_back(p); - continue; - } else { - // p1 is the only point of current which is also a point of the - // clipper. We can exit. - current.emplace_back(p1); - break; - } - } else { - if (det2 > eps) { - const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2); - current.emplace_back(p2); - current.emplace_back(p); - continue; - } else { - // p2 is the only point of current which is also a point of the - // clipper. We can exit. - current.emplace_back(p2); - break; - } - } - } else { - // - // Polygon-Polygon case. - // - std::fill(this->added_to_patch.begin(), // - this->added_to_patch.end(), // - false); - - const size_t previous_size = previous.size(); - for (size_t j = 0; j < previous_size; ++j) { - const Vec2f& p1 = previous[j]; - const Vec2f& p2 = previous[(j + 1) % previous_size]; - - const Vec2f ap1 = p1 - a; - const Vec2f ap2 = p2 - a; - - const FCL_REAL det1 = ab(0) * ap1(1) - ab(1) * ap1(0); - const FCL_REAL det2 = ab(0) * ap2(1) - ab(1) * ap2(0); - - if (det1 < 0 && det2 < 0) { - // No intersection. Continue to next segment of previous. - continue; - } - - if (det1 >= 0 && det2 >= 0) { - // Both p1 and p2 are inside the clipping polygon, add p1 to current - // (only if it has not already been added). - if (!this->added_to_patch[j]) { - current.emplace_back(p1); - this->added_to_patch[j] = true; - } - // Continue to next segment of previous. - continue; - } - - if (det1 >= 0) { - if (det1 > eps) { - if (!this->added_to_patch[j]) { - current.emplace_back(p1); - this->added_to_patch[j] = true; - } - const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2); - current.emplace_back(p); - } else { - // a, b and p1 are colinear; we add only p1. - if (!this->added_to_patch[j]) { - current.emplace_back(p1); - this->added_to_patch[j] = true; - } - } - } else { - if (det2 > eps) { - const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2); - current.emplace_back(p); - } else { - if (!this->added_to_patch[(j + 1) % previous.size()]) { - current.emplace_back(p2); - this->added_to_patch[(j + 1) % previous.size()] = true; - } - } - } - } - } - // - // End of iteration i of Sutherland-Hodgman. - if (current.size() <= 1) { - // No intersection or one point found, the algo can early stop. - break; - } - } - - // Transfer the result of the Sutherland-Hodgman algorithm to the contact - // patch. - this->getResult(contact, current_ptr, contact_patch); -} - -// ============================================================================ -inline void ContactPatchSolver::getResult( - const Contact& contact, const ContactPatch::Polygon* result_ptr, - ContactPatch& contact_patch) const { - if (result_ptr->size() <= 1) { - contact_patch.addPoint(contact.pos); - return; - } - - const ContactPatch::Polygon& result = *(result_ptr); - ContactPatch::Polygon& patch = contact_patch.points(); - patch = result; -} - -// ============================================================================ -template -inline void ContactPatchSolver::reset(const ShapeType1& shape1, - const Transform3f& tf1, - const ShapeType2& shape2, - const Transform3f& tf2, - const ContactPatch& contact_patch) const { - // Reset internal quantities - this->support_set_shape1.clear(); - this->support_set_shape2.clear(); - this->support_set_buffer.clear(); - - // Get the support function of each shape - const Transform3f& tfc = contact_patch.tf; - - this->support_set_shape1.direction = SupportSetDirection::DEFAULT; - // Set the reference frame of the support set of the first shape to be the - // local frame of shape 1. - Transform3f& tf1c = this->support_set_shape1.tf; - tf1c.rotation().noalias() = tf1.rotation().transpose() * tfc.rotation(); - tf1c.translation().noalias() = - tf1.rotation().transpose() * (tfc.translation() - tf1.translation()); - this->supportFuncShape1 = - this->makeSupportSetFunction(&shape1, this->supports_data[0]); - - this->support_set_shape2.direction = SupportSetDirection::INVERTED; - // Set the reference frame of the support set of the second shape to be the - // local frame of shape 2. - Transform3f& tf2c = this->support_set_shape2.tf; - tf2c.rotation().noalias() = tf2.rotation().transpose() * tfc.rotation(); - tf2c.translation().noalias() = - tf2.rotation().transpose() * (tfc.translation() - tf2.translation()); - this->supportFuncShape2 = - this->makeSupportSetFunction(&shape2, this->supports_data[1]); -} - -// ========================================================================== -inline Vec2f ContactPatchSolver::computeLineSegmentIntersection( - const Vec2f& a, const Vec2f& b, const Vec2f& c, const Vec2f& d) { - const Vec2f ab = b - a; - const Vec2f n(-ab(1), ab(0)); - const FCL_REAL denominator = n.dot(c - d); - if (std::abs(denominator) < std::numeric_limits::epsilon()) { - return d; - } - const FCL_REAL nominator = n.dot(a - d); - FCL_REAL alpha = nominator / denominator; - alpha = std::min(1.0, std::max(0.0, alpha)); - return alpha * c + (1 - alpha) * d; -} - -} // namespace fcl -} // namespace hpp - -#endif // HPP_FCL_CONTACT_PATCH_SOLVER_HXX +#include +#include diff --git a/include/hpp/fcl/contact_patch_func_matrix.h b/include/hpp/fcl/contact_patch_func_matrix.h index 7476650a4..9103090df 100644 --- a/include/hpp/fcl/contact_patch_func_matrix.h +++ b/include/hpp/fcl/contact_patch_func_matrix.h @@ -1,87 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2024, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of INRIA nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Louis Montaut */ - -#ifndef HPP_FCL_CONTACT_PATCH_FUNC_MATRIX_H -#define HPP_FCL_CONTACT_PATCH_FUNC_MATRIX_H - -#include "hpp/fcl/collision_data.h" -#include "hpp/fcl/contact_patch/contact_patch_solver.h" -#include "hpp/fcl/narrowphase/narrowphase.h" - -namespace hpp { -namespace fcl { - -/// @brief The contact patch matrix stores the functions for contact patches -/// computation between different types of objects and provides a uniform call -/// interface -struct HPP_FCL_DLLAPI ContactPatchFunctionMatrix { - /// @brief the uniform call interface for computing contact patches: we need - /// know - /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 - /// and tf2; - /// 2. the collision result that generated contact patches candidates - /// (`hpp::fcl::Contact`), from which contact patches will be expanded; - /// 3. the solver for computation of contact patches; - /// 4. the request setting for contact patches (e.g. maximum amount of - /// patches, patch tolerance etc.) - /// 5. the structure to return contact patches - /// (`hpp::fcl::ContactPatchResult`). - /// - /// Note: we pass a GJKSolver, because it allows to reuse internal computation - /// that was made during the narrow phase. It also allows to experiment with - /// different ways to compute contact patches. We could, for example, perturb - /// tf1 and tf2 and make multiple calls to the GJKSolver (although this is not - /// the approach done by default). - typedef void (*ContactPatchFunc)(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const CollisionResult& collision_result, - const ContactPatchSolver* csolver, - const ContactPatchRequest& request, - ContactPatchResult& result); - - /// @brief Each item in the contact patch matrix is a function to handle - /// contact patch computation between objects of type1 and type2. - ContactPatchFunc contact_patch_matrix[NODE_COUNT][NODE_COUNT]; - - ContactPatchFunctionMatrix(); -}; - -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/data_types.h b/include/hpp/fcl/data_types.h index bcbb3c9a3..690f179c6 100644 --- a/include/hpp/fcl/data_types.h +++ b/include/hpp/fcl/data_types.h @@ -1,189 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * Copyright (c) 2023, Inria - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_DATA_TYPES_H -#define HPP_FCL_DATA_TYPES_H - -#include -#include - -#include - -namespace hpp { - -#ifdef HPP_FCL_HAS_OCTOMAP -#define OCTOMAP_VERSION_AT_LEAST(x, y, z) \ - (OCTOMAP_MAJOR_VERSION > x || \ - (OCTOMAP_MAJOR_VERSION >= x && \ - (OCTOMAP_MINOR_VERSION > y || \ - (OCTOMAP_MINOR_VERSION >= y && OCTOMAP_PATCH_VERSION >= z)))) - -#define OCTOMAP_VERSION_AT_MOST(x, y, z) \ - (OCTOMAP_MAJOR_VERSION < x || \ - (OCTOMAP_MAJOR_VERSION <= x && \ - (OCTOMAP_MINOR_VERSION < y || \ - (OCTOMAP_MINOR_VERSION <= y && OCTOMAP_PATCH_VERSION <= z)))) -#endif // HPP_FCL_HAS_OCTOMAP -} // namespace hpp - -namespace hpp { -namespace fcl { -typedef double FCL_REAL; -typedef Eigen::Matrix Vec3f; -typedef Eigen::Matrix Vec2f; -typedef Eigen::Matrix Vec6f; -typedef Eigen::Matrix VecXf; -typedef Eigen::Matrix Matrix3f; -typedef Eigen::Matrix Matrixx3f; -typedef Eigen::Matrix Matrixx2f; -typedef Eigen::Matrix - Matrixx3i; -typedef Eigen::Matrix MatrixXf; -typedef Eigen::Vector2i support_func_guess_t; - -/// @brief Initial guess to use for the GJK algorithm -/// DefaultGuess: Vec3f(1, 0, 0) -/// CachedGuess: previous vector found by GJK or guess cached by the user -/// BoundingVolumeGuess: guess using the centers of the shapes' AABB -/// WARNING: to use BoundingVolumeGuess, computeLocalAABB must have been called -/// on the two shapes. -enum GJKInitialGuess { DefaultGuess, CachedGuess, BoundingVolumeGuess }; - -/// @brief Variant to use for the GJK algorithm -enum GJKVariant { DefaultGJK, PolyakAcceleration, NesterovAcceleration }; - -/// @brief Which convergence criterion is used to stop the algorithm (when the -/// shapes are not in collision). (default) VDB: Van den Bergen (A Fast and -/// Robust GJK Implementation, 1999) DG: duality-gap, as used in the Frank-Wolfe -/// and the vanilla 1988 GJK algorithms Hybrid: a mix between VDB and DG. -enum GJKConvergenceCriterion { Default, DualityGap, Hybrid }; - -/// @brief Wether the convergence criterion is scaled on the norm of the -/// solution or not -enum GJKConvergenceCriterionType { Relative, Absolute }; - -/// @brief Triangle with 3 indices for points -class HPP_FCL_DLLAPI Triangle { - public: - typedef std::size_t index_type; - typedef int size_type; - - /// @brief Default constructor - Triangle() {} - - /// @brief Create a triangle with given vertex indices - Triangle(index_type p1, index_type p2, index_type p3) { set(p1, p2, p3); } - - /// @brief Set the vertex indices of the triangle - inline void set(index_type p1, index_type p2, index_type p3) { - vids[0] = p1; - vids[1] = p2; - vids[2] = p3; - } - - /// @brief Access the triangle index - inline index_type operator[](index_type i) const { return vids[i]; } - - inline index_type& operator[](index_type i) { return vids[i]; } - - static inline size_type size() { return 3; } - - bool operator==(const Triangle& other) const { - return vids[0] == other.vids[0] && vids[1] == other.vids[1] && - vids[2] == other.vids[2]; - } - - bool operator!=(const Triangle& other) const { return !(*this == other); } - - bool isValid() const { - return vids[0] != (std::numeric_limits::max)() && - vids[1] != (std::numeric_limits::max)() && - vids[2] != (std::numeric_limits::max)(); - } - - private: - /// @brief indices for each vertex of triangle - index_type vids[3] = {(std::numeric_limits::max)(), - (std::numeric_limits::max)(), - (std::numeric_limits::max)()}; -}; - -/// @brief Quadrilateral with 4 indices for points -struct HPP_FCL_DLLAPI Quadrilateral { - typedef std::size_t index_type; - typedef int size_type; - - Quadrilateral() {} - - Quadrilateral(index_type p0, index_type p1, index_type p2, index_type p3) { - set(p0, p1, p2, p3); - } - - /// @brief Set the vertex indices of the quadrilateral - inline void set(index_type p0, index_type p1, index_type p2, index_type p3) { - vids[0] = p0; - vids[1] = p1; - vids[2] = p2; - vids[3] = p3; - } - - /// @access the quadrilateral index - inline index_type operator[](index_type i) const { return vids[i]; } - - inline index_type& operator[](index_type i) { return vids[i]; } - - static inline size_type size() { return 4; } - - bool operator==(const Quadrilateral& other) const { - return vids[0] == other.vids[0] && vids[1] == other.vids[1] && - vids[2] == other.vids[2] && vids[3] == other.vids[3]; - } - - bool operator!=(const Quadrilateral& other) const { - return !(*this == other); - } - - private: - index_type vids[4]; -}; - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/deprecated.hh b/include/hpp/fcl/deprecated.hh new file mode 100644 index 000000000..513abdf65 --- /dev/null +++ b/include/hpp/fcl/deprecated.hh @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/distance.h b/include/hpp/fcl/distance.h index c0c456494..a8301218b 100644 --- a/include/hpp/fcl/distance.h +++ b/include/hpp/fcl/distance.h @@ -1,117 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef HPP_FCL_DISTANCE_H -#define HPP_FCL_DISTANCE_H - -#include -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// @brief Main distance interface: given two collision objects, and the -/// requirements for contacts, including whether return the nearest points, this -/// function performs the distance between them. Return value is the minimum -/// distance generated between the two objects. -HPP_FCL_DLLAPI FCL_REAL distance(const CollisionObject* o1, - const CollisionObject* o2, - const DistanceRequest& request, - DistanceResult& result); - -/// @copydoc distance(const CollisionObject*, const CollisionObject*, const -/// DistanceRequest&, DistanceResult&) -HPP_FCL_DLLAPI FCL_REAL distance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result); - -/// This class reduces the cost of identifying the geometry pair. -/// This is mostly useful for repeated shape-shape queries. -/// -/// \code -/// ComputeDistance calc_distance (o1, o2); -/// FCL_REAL distance = calc_distance(tf1, tf2, request, result); -/// \endcode -class HPP_FCL_DLLAPI ComputeDistance { - public: - ComputeDistance(const CollisionGeometry* o1, const CollisionGeometry* o2); - - FCL_REAL operator()(const Transform3f& tf1, const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result) const; - - bool operator==(const ComputeDistance& other) const { - return o1 == other.o1 && o2 == other.o2 && swap_geoms == other.swap_geoms && - solver == other.solver && func == other.func; - } - - bool operator!=(const ComputeDistance& other) const { - return !(*this == other); - } - - virtual ~ComputeDistance() {}; - - protected: - // These pointers are made mutable to let the derived classes to update - // their values when updating the collision geometry (e.g. creating a new - // one). This feature should be used carefully to avoid any mis usage (e.g, - // changing the type of the collision geometry should be avoided). - mutable const CollisionGeometry* o1; - mutable const CollisionGeometry* o2; - - mutable GJKSolver solver; - - DistanceFunctionMatrix::DistanceFunc func; - bool swap_geoms; - - virtual FCL_REAL run(const Transform3f& tf1, const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result) const; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/distance_func_matrix.h b/include/hpp/fcl/distance_func_matrix.h index cf51cb906..5bc9be51c 100644 --- a/include/hpp/fcl/distance_func_matrix.h +++ b/include/hpp/fcl/distance_func_matrix.h @@ -1,77 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_DISTANCE_FUNC_MATRIX_H -#define HPP_FCL_DISTANCE_FUNC_MATRIX_H - -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// @brief distance matrix stores the functions for distance between different -/// types of objects and provides a uniform call interface -struct HPP_FCL_DLLAPI DistanceFunctionMatrix { - /// @brief the uniform call interface for distance: for distance, we need know - /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 - /// and tf2; - /// 2. the solver for narrow phase collision, this is for distance computation - /// between geometric shapes; - /// 3. the request setting for distance (e.g., whether need to return nearest - /// points); - typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result); - - /// @brief each item in the distance matrix is a function to handle distance - /// between objects of type1 and type2 - DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT]; - - DistanceFunctionMatrix(); -}; - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/fwd.hh b/include/hpp/fcl/fwd.hh index cb756a84e..1bf07ff6c 100644 --- a/include/hpp/fcl/fwd.hh +++ b/include/hpp/fcl/fwd.hh @@ -1,151 +1,2 @@ -// -// Software License Agreement (BSD License) -// -// Copyright (c) 2014, CNRS-LAAS -// Copyright (c) 2022-2024, Inria -// Author: Florent Lamiraux -// -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. -// * Neither the name of CNRS-LAAS nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// - -#ifndef HPP_FCL_FWD_HH -#define HPP_FCL_FWD_HH - -#include -#include -#include -#include - -#include -#include -#include - -#if _WIN32 -#define HPP_FCL_PRETTY_FUNCTION __FUNCSIG__ -#else -#define HPP_FCL_PRETTY_FUNCTION __PRETTY_FUNCTION__ -#endif - -#define HPP_FCL_UNUSED_VARIABLE(var) (void)(var) - -#ifdef NDEBUG -#define HPP_FCL_ONLY_USED_FOR_DEBUG(var) HPP_FCL_UNUSED_VARIABLE(var) -#else -#define HPP_FCL_ONLY_USED_FOR_DEBUG(var) -#endif - -#define HPP_FCL_THROW_PRETTY(message, exception) \ - { \ - std::stringstream ss; \ - ss << "From file: " << __FILE__ << "\n"; \ - ss << "in function: " << HPP_FCL_PRETTY_FUNCTION << "\n"; \ - ss << "at line: " << __LINE__ << "\n"; \ - ss << "message: " << message << "\n"; \ - throw exception(ss.str()); \ - } - -#ifdef HPP_FCL_TURN_ASSERT_INTO_EXCEPTION -#define HPP_FCL_ASSERT(check, message, exception) \ - do { \ - if (!(check)) { \ - HPP_FCL_THROW_PRETTY(message, exception); \ - } \ - } while (0) -#else -#define HPP_FCL_ASSERT(check, message, exception) \ - { \ - HPP_FCL_UNUSED_VARIABLE(exception(message)); \ - assert((check) && message); \ - } -#endif - -#if (__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) -#define HPP_FCL_WITH_CXX11_SUPPORT -#endif - -#if defined(__GNUC__) || defined(__clang__) -#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH _Pragma("GCC diagnostic push") -#define HPP_FCL_COMPILER_DIAGNOSTIC_POP _Pragma("GCC diagnostic pop") -#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \ - _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"") - -// GCC version 4.6 and higher supports -Wmaybe-uninitialized -#if (defined(__GNUC__) && \ - ((__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6))) -#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \ - _Pragma("GCC diagnostic ignored \"-Wmaybe-uninitialized\"") -// Use __has_warning with clang. Clang introduced it in 2024 (3.5+) -#elif (defined(__clang__) && defined(__has_warning) && \ - __has_warning("-Wmaybe-uninitialized")) -#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \ - _Pragma("clang diagnostic ignored \"-Wmaybe-uninitialized\"") -#else -#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED -#endif -#elif defined(WIN32) -#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH _Pragma("warning(push)") -#define HPP_FCL_COMPILER_DIAGNOSTIC_POP _Pragma("warning(pop)") -#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \ - _Pragma("warning(disable : 4996)") -#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \ - _Pragma("warning(disable : 4700)") -#else -#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH -#define HPP_FCL_COMPILER_DIAGNOSTIC_POP -#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS -#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED -#endif // __GNUC__ - -namespace hpp { -namespace fcl { -using std::dynamic_pointer_cast; -using std::make_shared; -using std::shared_ptr; - -class CollisionObject; -typedef shared_ptr CollisionObjectPtr_t; -typedef shared_ptr CollisionObjectConstPtr_t; -class CollisionGeometry; -typedef shared_ptr CollisionGeometryPtr_t; -typedef shared_ptr CollisionGeometryConstPtr_t; -class Transform3f; - -class AABB; - -class BVHModelBase; -typedef shared_ptr BVHModelPtr_t; - -class OcTree; -typedef shared_ptr OcTreePtr_t; -typedef shared_ptr OcTreeConstPtr_t; -} // namespace fcl -} // namespace hpp - -#endif // HPP_FCL_FWD_HH +#include +#include diff --git a/include/hpp/fcl/hfield.h b/include/hpp/fcl/hfield.h index 18ab9e5a1..4d462776a 100644 --- a/include/hpp/fcl/hfield.h +++ b/include/hpp/fcl/hfield.h @@ -1,545 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2021-2024, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Justin Carpentier */ - -#ifndef HPP_FCL_HEIGHT_FIELD_H -#define HPP_FCL_HEIGHT_FIELD_H - -#include "hpp/fcl/fwd.hh" -#include "hpp/fcl/data_types.h" -#include "hpp/fcl/collision_object.h" -#include "hpp/fcl/BV/BV_node.h" -#include "hpp/fcl/BVH/BVH_internal.h" - -#include - -namespace hpp { -namespace fcl { - -/// @addtogroup Construction_Of_HeightField -/// @{ - -struct HPP_FCL_DLLAPI HFNodeBase { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - enum class FaceOrientation { - TOP = 1, - BOTTOM = 1, - NORTH = 2, - EAST = 4, - SOUTH = 8, - WEST = 16 - }; - - /// @brief An index for first child node or primitive - /// If the value is positive, it is the index of the first child bv node - /// If the value is negative, it is -(primitive index + 1) - /// Zero is not used. - size_t first_child; - - Eigen::DenseIndex x_id, x_size; - Eigen::DenseIndex y_id, y_size; - - FCL_REAL max_height; - int contact_active_faces; - - /// @brief Default constructor - HFNodeBase() - : first_child(0), - x_id(-1), - x_size(0), - y_id(-1), - y_size(0), - max_height(std::numeric_limits::lowest()), - contact_active_faces(0) {} - - /// @brief Comparison operator - bool operator==(const HFNodeBase& other) const { - return first_child == other.first_child && x_id == other.x_id && - x_size == other.x_size && y_id == other.y_id && - y_size == other.y_size && max_height == other.max_height && - contact_active_faces == other.contact_active_faces; - } - - /// @brief Difference operator - bool operator!=(const HFNodeBase& other) const { return !(*this == other); } - - /// @brief Whether current node is a leaf node (i.e. contains a primitive - /// index) - inline bool isLeaf() const { return x_size == 1 && y_size == 1; } - - /// @brief Return the index of the first child. The index is referred to the - /// bounding volume array (i.e. bvs) in BVHModel - inline size_t leftChild() const { return first_child; } - - /// @brief Return the index of the second child. The index is referred to the - /// bounding volume array (i.e. bvs) in BVHModel - inline size_t rightChild() const { return first_child + 1; } - - inline Eigen::Vector2i leftChildIndexes() const { - return Eigen::Vector2i(x_id, y_id); - } - inline Eigen::Vector2i rightChildIndexes() const { - return Eigen::Vector2i(x_id + x_size / 2, y_id + y_size / 2); - } -}; - -inline HFNodeBase::FaceOrientation operator&(HFNodeBase::FaceOrientation a, - HFNodeBase::FaceOrientation b) { - return HFNodeBase::FaceOrientation(int(a) & int(b)); -} - -inline int operator&(int a, HFNodeBase::FaceOrientation b) { - return a & int(b); -} - -template -struct HPP_FCL_DLLAPI HFNode : public HFNodeBase { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - typedef HFNodeBase Base; - - /// @brief bounding volume storing the geometry - BV bv; - - /// @brief Equality operator - bool operator==(const HFNode& other) const { - return Base::operator==(other) && bv == other.bv; - } - - /// @brief Difference operator - bool operator!=(const HFNode& other) const { return !(*this == other); } - - /// @brief Check whether two BVNode collide - bool overlap(const HFNode& other) const { return bv.overlap(other.bv); } - /// @brief Check whether two BVNode collide - bool overlap(const HFNode& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const { - return bv.overlap(other.bv, request, sqrDistLowerBound); - } - - /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and - /// the underlying BV supports distance, return the nearest points. - FCL_REAL distance(const HFNode& other, Vec3f* P1 = NULL, - Vec3f* P2 = NULL) const { - return bv.distance(other.bv, P1, P2); - } - - /// @brief Access to the center of the BV - Vec3f getCenter() const { return bv.center(); } - - /// @brief Access to the orientation of the BV - hpp::fcl::Matrix3f::IdentityReturnType getOrientation() const { - return Matrix3f::Identity(); - } - - virtual ~HFNode() {} -}; - -namespace details { - -template -struct UpdateBoundingVolume { - static void run(const Vec3f& pointA, const Vec3f& pointB, BV& bv) { - AABB bv_aabb(pointA, pointB); - // AABB bv_aabb; - // bv_aabb.update(pointA,pointB); - convertBV(bv_aabb, bv); - } -}; - -template <> -struct UpdateBoundingVolume { - static void run(const Vec3f& pointA, const Vec3f& pointB, AABB& bv) { - AABB bv_aabb(pointA, pointB); - convertBV(bv_aabb, bv); - // bv.update(pointA,pointB); - } -}; - -} // namespace details - -/// @brief Data structure depicting a height field given by the base grid -/// dimensions and the elevation along the grid. \tparam BV one of the bounding -/// volume class in \ref Bounding_Volume. -/// -/// An height field is defined by its base dimensions along the X and Y axes and -/// a set ofpoints defined by their altitude, regularly dispatched on the grid. -/// The height field is centered at the origin and the corners of the geometry -/// correspond to the following coordinates [± x_dim/2; ± y_dim/2]. -template -class HPP_FCL_DLLAPI HeightField : public CollisionGeometry { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - typedef CollisionGeometry Base; - - typedef HFNode Node; - typedef std::vector> BVS; - - /// @brief Constructing an empty HeightField - HeightField() - : CollisionGeometry(), - min_height((std::numeric_limits::min)()), - max_height((std::numeric_limits::max)()) {} - - /// @brief Constructing an HeightField from its base dimensions and the set of - /// heights points. - /// The granularity of the height field along X and Y direction is - /// extraded from the Z grid. - /// - /// \param[in] x_dim Dimension along the X axis - /// \param[in] y_dim Dimension along the Y axis - /// \param[in] heights Matrix containing the altitude of each point compositng - /// the height field - /// \param[in] min_height Minimal height of the height field - /// - HeightField(const FCL_REAL x_dim, const FCL_REAL y_dim, - const MatrixXf& heights, const FCL_REAL min_height = (FCL_REAL)0) - : CollisionGeometry() { - init(x_dim, y_dim, heights, min_height); - } - - /// @brief Copy contructor from another HeightField - /// - /// \param[in] other to copy. - /// - HeightField(const HeightField& other) - : CollisionGeometry(other), - x_dim(other.x_dim), - y_dim(other.y_dim), - heights(other.heights), - min_height(other.min_height), - max_height(other.max_height), - x_grid(other.x_grid), - y_grid(other.y_grid), - bvs(other.bvs), - num_bvs(other.num_bvs) {} - - /// @brief Returns a const reference of the grid along the X direction. - const VecXf& getXGrid() const { return x_grid; } - /// @brief Returns a const reference of the grid along the Y direction. - const VecXf& getYGrid() const { return y_grid; } - - /// @brief Returns a const reference of the heights - const MatrixXf& getHeights() const { return heights; } - - /// @brief Returns the dimension of the Height Field along the X direction. - FCL_REAL getXDim() const { return x_dim; } - /// @brief Returns the dimension of the Height Field along the Y direction. - FCL_REAL getYDim() const { return y_dim; } - - /// @brief Returns the minimal height value of the Height Field. - FCL_REAL getMinHeight() const { return min_height; } - /// @brief Returns the maximal height value of the Height Field. - FCL_REAL getMaxHeight() const { return max_height; } - - virtual HeightField* clone() const { return new HeightField(*this); } - - const BVS& getNodes() const { return bvs; } - - /// @brief deconstruction, delete mesh data related. - virtual ~HeightField() {} - - /// @brief Compute the AABB for the HeightField, used for broad-phase - /// collision - void computeLocalAABB() { - const Vec3f A(x_grid[0], y_grid[0], min_height); - const Vec3f B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1], - max_height); - const AABB aabb_(A, B); - - aabb_radius = (A - B).norm() / 2.; - aabb_local = aabb_; - aabb_center = aabb_.center(); - } - - /// @brief Update Height Field height - void updateHeights(const MatrixXf& new_heights) { - if (new_heights.rows() != heights.rows() || - new_heights.cols() != heights.cols()) - HPP_FCL_THROW_PRETTY( - "The matrix containing the new heights values does not have the same " - "matrix size as the original one.\n" - "\tinput values - rows: " - << new_heights.rows() << " - cols: " << new_heights.cols() << "\n" - << "\texpected values - rows: " << heights.rows() - << " - cols: " << heights.cols() << "\n", - std::invalid_argument); - - heights = new_heights.cwiseMax(min_height); - this->max_height = recursiveUpdateHeight(0); - assert(this->max_height == heights.maxCoeff()); - } - - protected: - void init(const FCL_REAL x_dim, const FCL_REAL y_dim, const MatrixXf& heights, - const FCL_REAL min_height) { - this->x_dim = x_dim; - this->y_dim = y_dim; - this->heights = heights.cwiseMax(min_height); - this->min_height = min_height; - this->max_height = heights.maxCoeff(); - - const Eigen::DenseIndex NX = heights.cols(), NY = heights.rows(); - assert(NX >= 2 && "The number of columns is too small."); - assert(NY >= 2 && "The number of rows is too small."); - - x_grid = VecXf::LinSpaced(NX, -0.5 * x_dim, 0.5 * x_dim); - y_grid = VecXf::LinSpaced(NY, 0.5 * y_dim, -0.5 * y_dim); - - // Allocate BVS - const size_t num_tot_bvs = - (size_t)(NX * NY) - 1 + (size_t)((NX - 1) * (NY - 1)); - bvs.resize(num_tot_bvs); - num_bvs = 0; - - // Build tree - buildTree(); - } - - /// @brief Get the object type: it is a HFIELD - OBJECT_TYPE getObjectType() const { return OT_HFIELD; } - - Vec3f computeCOM() const { return Vec3f::Zero(); } - - FCL_REAL computeVolume() const { return 0; } - - Matrix3f computeMomentofInertia() const { return Matrix3f::Zero(); } - - protected: - /// @brief Dimensions in meters along X and Y directions - FCL_REAL x_dim, y_dim; - - /// @brief Elevation values in meters of the Height Field - MatrixXf heights; - - /// @brief Minimal height of the Height Field: all values bellow min_height - /// will be discarded. - FCL_REAL min_height, max_height; - - /// @brief Grids along the X and Y directions. Useful for plotting or other - /// related things. - VecXf x_grid, y_grid; - - /// @brief Bounding volume hierarchy - BVS bvs; - unsigned int num_bvs; - - /// @brief Build the bounding volume hierarchy - int buildTree() { - num_bvs = 1; - const FCL_REAL max_recursive_height = - recursiveBuildTree(0, 0, heights.cols() - 1, 0, heights.rows() - 1); - assert(max_recursive_height == max_height && - "the maximal height is not correct"); - HPP_FCL_UNUSED_VARIABLE(max_recursive_height); - - bvs.resize(num_bvs); - return BVH_OK; - } - - FCL_REAL recursiveUpdateHeight(const size_t bv_id) { - HFNode& bv_node = bvs[bv_id]; - - FCL_REAL max_height; - if (bv_node.isLeaf()) { - max_height = heights.block<2, 2>(bv_node.y_id, bv_node.x_id).maxCoeff(); - } else { - FCL_REAL - max_left_height = recursiveUpdateHeight(bv_node.leftChild()), - max_right_height = recursiveUpdateHeight(bv_node.rightChild()); - - max_height = (std::max)(max_left_height, max_right_height); - } - - bv_node.max_height = max_height; - - const Vec3f pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height); - const Vec3f pointB(x_grid[bv_node.x_id + bv_node.x_size], - y_grid[bv_node.y_id + bv_node.y_size], max_height); - - details::UpdateBoundingVolume::run(pointA, pointB, bv_node.bv); - - return max_height; - } - - FCL_REAL recursiveBuildTree(const size_t bv_id, const Eigen::DenseIndex x_id, - const Eigen::DenseIndex x_size, - const Eigen::DenseIndex y_id, - const Eigen::DenseIndex y_size) { - assert(x_id < heights.cols() && "x_id is out of bounds"); - assert(y_id < heights.rows() && "y_id is out of bounds"); - assert(x_size >= 0 && y_size >= 0 && - "x_size or y_size are not of correct value"); - assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension"); - - HFNode& bv_node = bvs[bv_id]; - FCL_REAL max_height; - if (x_size == 1 && - y_size == 1) // don't build any BV for the current child node - { - max_height = heights.block<2, 2>(y_id, x_id).maxCoeff(); - } else { - bv_node.first_child = num_bvs; - num_bvs += 2; - - FCL_REAL max_left_height = min_height, max_right_height = min_height; - if (x_size >= y_size) // splitting along the X axis - { - Eigen::DenseIndex x_size_half = x_size / 2; - if (x_size == 1) x_size_half = 1; - max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, - x_size_half, y_id, y_size); - - max_right_height = - recursiveBuildTree(bv_node.rightChild(), x_id + x_size_half, - x_size - x_size_half, y_id, y_size); - } else // splitting along the Y axis - { - Eigen::DenseIndex y_size_half = y_size / 2; - if (y_size == 1) y_size_half = 1; - max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size, - y_id, y_size_half); - - max_right_height = - recursiveBuildTree(bv_node.rightChild(), x_id, x_size, - y_id + y_size_half, y_size - y_size_half); - } - - max_height = (std::max)(max_left_height, max_right_height); - } - - bv_node.max_height = max_height; - // max_height = std::max(max_height,min_height); - - const Vec3f pointA(x_grid[x_id], y_grid[y_id], min_height); - assert(x_id + x_size < x_grid.size()); - assert(y_id + y_size < y_grid.size()); - const Vec3f pointB(x_grid[x_id + x_size], y_grid[y_id + y_size], - max_height); - - details::UpdateBoundingVolume::run(pointA, pointB, bv_node.bv); - bv_node.x_id = x_id; - bv_node.y_id = y_id; - bv_node.x_size = x_size; - bv_node.y_size = y_size; - - if (bv_node.isLeaf()) { - int& contact_active_faces = bv_node.contact_active_faces; - contact_active_faces |= int(HFNodeBase::FaceOrientation::TOP); - contact_active_faces |= int(HFNodeBase::FaceOrientation::BOTTOM); - - if (bv_node.x_id == 0) // first col - contact_active_faces |= int(HFNodeBase::FaceOrientation::WEST); - - if (bv_node.y_id == 0) // first row (TOP) - contact_active_faces |= int(HFNodeBase::FaceOrientation::NORTH); - - if (bv_node.x_id + 1 == heights.cols() - 1) // last col - contact_active_faces |= int(HFNodeBase::FaceOrientation::EAST); - - if (bv_node.y_id + 1 == heights.rows() - 1) // last row (BOTTOM) - contact_active_faces |= int(HFNodeBase::FaceOrientation::SOUTH); - } - - return max_height; - } - - public: - /// @brief Access the bv giving the its index - const HFNode& getBV(unsigned int i) const { - if (i >= num_bvs) - HPP_FCL_THROW_PRETTY("Index out of bounds", std::invalid_argument); - return bvs[i]; - } - - /// @brief Access the bv giving the its index - HFNode& getBV(unsigned int i) { - if (i >= num_bvs) - HPP_FCL_THROW_PRETTY("Index out of bounds", std::invalid_argument); - return bvs[i]; - } - - /// @brief Get the BV type: default is unknown - NODE_TYPE getNodeType() const { return BV_UNKNOWN; } - - private: - virtual bool isEqual(const CollisionGeometry& _other) const { - const HeightField* other_ptr = dynamic_cast(&_other); - if (other_ptr == nullptr) return false; - const HeightField& other = *other_ptr; - - return x_dim == other.x_dim && y_dim == other.y_dim && - heights == other.heights && min_height == other.min_height && - max_height == other.max_height && x_grid == other.x_grid && - y_grid == other.y_grid && bvs == other.bvs && - num_bvs == other.num_bvs; - } -}; - -/// @brief Specialization of getNodeType() for HeightField with different BV -/// types -template <> -NODE_TYPE HeightField::getNodeType() const; - -template <> -NODE_TYPE HeightField::getNodeType() const; - -template <> -NODE_TYPE HeightField::getNodeType() const; - -template <> -NODE_TYPE HeightField::getNodeType() const; - -template <> -NODE_TYPE HeightField::getNodeType() const; - -template <> -NODE_TYPE HeightField>::getNodeType() const; - -template <> -NODE_TYPE HeightField>::getNodeType() const; - -template <> -NODE_TYPE HeightField>::getNodeType() const; - -/// @} - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/internal/BV_fitter.h b/include/hpp/fcl/internal/BV_fitter.h index 514741f0b..438bd599e 100644 --- a/include/hpp/fcl/internal/BV_fitter.h +++ b/include/hpp/fcl/internal/BV_fitter.h @@ -1,223 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_BV_FITTER_H -#define HPP_FCL_BV_FITTER_H - -#include -#include -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// @brief Compute a bounding volume that fits a set of n points. -template -void fit(Vec3f* ps, unsigned int n, BV& bv) { - for (unsigned int i = 0; i < n; ++i) // TODO(jcarpent): vectorize - { - bv += ps[i]; - } -} - -template <> -void fit(Vec3f* ps, unsigned int n, OBB& bv); - -template <> -void fit(Vec3f* ps, unsigned int n, RSS& bv); - -template <> -void fit(Vec3f* ps, unsigned int n, kIOS& bv); - -template <> -void fit(Vec3f* ps, unsigned int n, OBBRSS& bv); - -template <> -void fit(Vec3f* ps, unsigned int n, AABB& bv); - -/// @brief The class for the default algorithm fitting a bounding volume to a -/// set of points -template -class HPP_FCL_DLLAPI BVFitterTpl { - public: - /// @brief default deconstructor - virtual ~BVFitterTpl() {} - - /// @brief Prepare the geometry primitive data for fitting - void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) { - vertices = vertices_; - prev_vertices = NULL; - tri_indices = tri_indices_; - type = type_; - } - - /// @brief Prepare the geometry primitive data for fitting, for deformable - /// mesh - void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, - BVHModelType type_) { - vertices = vertices_; - prev_vertices = prev_vertices_; - tri_indices = tri_indices_; - type = type_; - } - - /// @brief Compute the fitting BV - virtual BV fit(unsigned int* primitive_indices, - unsigned int num_primitives) = 0; - - /// @brief Clear the geometry primitive data - void clear() { - vertices = NULL; - prev_vertices = NULL; - tri_indices = NULL; - type = BVH_MODEL_UNKNOWN; - } - - protected: - Vec3f* vertices; - Vec3f* prev_vertices; - Triangle* tri_indices; - BVHModelType type; -}; - -/// @brief The class for the default algorithm fitting a bounding volume to a -/// set of points -template -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { - typedef BVFitterTpl Base; - - public: - /// @brief Compute a bounding volume that fits a set of primitives (points or - /// triangles). The primitive data was set by set function and - /// primitive_indices is the primitive index relative to the data - BV fit(unsigned int* primitive_indices, unsigned int num_primitives) { - BV bv; - - if (type == BVH_MODEL_TRIANGLES) /// The primitive is triangle - { - for (unsigned int i = 0; i < num_primitives; ++i) { - Triangle t = tri_indices[primitive_indices[i]]; - bv += vertices[t[0]]; - bv += vertices[t[1]]; - bv += vertices[t[2]]; - - if (prev_vertices) /// can fitting both current and previous frame - { - bv += prev_vertices[t[0]]; - bv += prev_vertices[t[1]]; - bv += prev_vertices[t[2]]; - } - } - } else if (type == BVH_MODEL_POINTCLOUD) /// The primitive is point - { - for (unsigned int i = 0; i < num_primitives; ++i) { - bv += vertices[primitive_indices[i]]; - - if (prev_vertices) /// can fitting both current and previous frame - { - bv += prev_vertices[primitive_indices[i]]; - } - } - } - - return bv; - } - - protected: - using Base::prev_vertices; - using Base::tri_indices; - using Base::type; - using Base::vertices; -}; - -/// @brief Specification of BVFitter for OBB bounding volume -template <> -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { - public: - /// @brief Compute a bounding volume that fits a set of primitives (points or - /// triangles). The primitive data was set by set function and - /// primitive_indices is the primitive index relative to the data. - OBB fit(unsigned int* primitive_indices, unsigned int num_primitives); -}; - -/// @brief Specification of BVFitter for RSS bounding volume -template <> -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { - public: - /// @brief Compute a bounding volume that fits a set of primitives (points or - /// triangles). The primitive data was set by set function and - /// primitive_indices is the primitive index relative to the data. - RSS fit(unsigned int* primitive_indices, unsigned int num_primitives); -}; - -/// @brief Specification of BVFitter for kIOS bounding volume -template <> -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { - public: - /// @brief Compute a bounding volume that fits a set of primitives (points or - /// triangles). The primitive data was set by set function and - /// primitive_indices is the primitive index relative to the data. - kIOS fit(unsigned int* primitive_indices, unsigned int num_primitives); -}; - -/// @brief Specification of BVFitter for OBBRSS bounding volume -template <> -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { - public: - /// @brief Compute a bounding volume that fits a set of primitives (points or - /// triangles). The primitive data was set by set function and - /// primitive_indices is the primitive index relative to the data. - OBBRSS fit(unsigned int* primitive_indices, unsigned int num_primitives); -}; - -/// @brief Specification of BVFitter for AABB bounding volume -template <> -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { - public: - /// @brief Compute a bounding volume that fits a set of primitives (points or - /// triangles). The primitive data was set by set function and - /// primitive_indices is the primitive index relative to the data. - AABB fit(unsigned int* primitive_indices, unsigned int num_primitives); -}; - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/internal/BV_splitter.h b/include/hpp/fcl/internal/BV_splitter.h index 24c5c6077..85a586bdb 100644 --- a/include/hpp/fcl/internal/BV_splitter.h +++ b/include/hpp/fcl/internal/BV_splitter.h @@ -1,286 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_BV_SPLITTER_H -#define HPP_FCL_BV_SPLITTER_H - -#include -#include -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// @brief Three types of split algorithms are provided in FCL as default -enum SplitMethodType { - SPLIT_METHOD_MEAN, - SPLIT_METHOD_MEDIAN, - SPLIT_METHOD_BV_CENTER -}; - -/// @brief A class describing the split rule that splits each BV node -template -class BVSplitter { - public: - BVSplitter(SplitMethodType method) - : split_vector(0, 0, 0), split_method(method) {} - - /// @brief Default deconstructor - virtual ~BVSplitter() {} - - /// @brief Set the geometry data needed by the split rule - void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) { - vertices = vertices_; - tri_indices = tri_indices_; - type = type_; - } - - /// @brief Compute the split rule according to a subset of geometry and the - /// corresponding BV node - void computeRule(const BV& bv, unsigned int* primitive_indices, - unsigned int num_primitives) { - switch (split_method) { - case SPLIT_METHOD_MEAN: - computeRule_mean(bv, primitive_indices, num_primitives); - break; - case SPLIT_METHOD_MEDIAN: - computeRule_median(bv, primitive_indices, num_primitives); - break; - case SPLIT_METHOD_BV_CENTER: - computeRule_bvcenter(bv, primitive_indices, num_primitives); - break; - default: - std::cerr << "Split method not supported" << std::endl; - } - } - - /// @brief Apply the split rule on a given point - bool apply(const Vec3f& q) const { return q[split_axis] > split_value; } - - /// @brief Clear the geometry data set before - void clear() { - vertices = NULL; - tri_indices = NULL; - type = BVH_MODEL_UNKNOWN; - } - - protected: - /// @brief The axis based on which the split decision is made. For most BV, - /// the axis is aligned with one of the world coordinate, so only split_axis - /// is needed. For oriented node, we can use a vector to make a better split - /// decision. - int split_axis; - Vec3f split_vector; - - /// @brief The split threshold, different primitives are splitted according - /// whether their projection on the split_axis is larger or smaller than the - /// threshold - FCL_REAL split_value; - - /// @brief The mesh vertices or points handled by the splitter - Vec3f* vertices; - - /// @brief The triangles handled by the splitter - Triangle* tri_indices; - - /// @brief Whether the geometry is mesh or point cloud - BVHModelType type; - - /// @brief The split algorithm used - SplitMethodType split_method; - - /// @brief Split algorithm 1: Split the node from center - void computeRule_bvcenter(const BV& bv, unsigned int*, unsigned int) { - Vec3f center = bv.center(); - int axis = 2; - - if (bv.width() >= bv.height() && bv.width() >= bv.depth()) - axis = 0; - else if (bv.height() >= bv.width() && bv.height() >= bv.depth()) - axis = 1; - - split_axis = axis; - split_value = center[axis]; - } - - /// @brief Split algorithm 2: Split the node according to the mean of the data - /// contained - void computeRule_mean(const BV& bv, unsigned int* primitive_indices, - unsigned int num_primitives) { - int axis = 2; - - if (bv.width() >= bv.height() && bv.width() >= bv.depth()) - axis = 0; - else if (bv.height() >= bv.width() && bv.height() >= bv.depth()) - axis = 1; - - split_axis = axis; - FCL_REAL sum = 0; - - if (type == BVH_MODEL_TRIANGLES) { - for (unsigned int i = 0; i < num_primitives; ++i) { - const Triangle& t = tri_indices[primitive_indices[i]]; - sum += (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + - vertices[t[2]][split_axis]); - } - - sum /= 3; - } else if (type == BVH_MODEL_POINTCLOUD) { - for (unsigned int i = 0; i < num_primitives; ++i) { - sum += vertices[primitive_indices[i]][split_axis]; - } - } - - split_value = sum / num_primitives; - } - - /// @brief Split algorithm 3: Split the node according to the median of the - /// data contained - void computeRule_median(const BV& bv, unsigned int* primitive_indices, - unsigned int num_primitives) { - int axis = 2; - - if (bv.width() >= bv.height() && bv.width() >= bv.depth()) - axis = 0; - else if (bv.height() >= bv.width() && bv.height() >= bv.depth()) - axis = 1; - - split_axis = axis; - std::vector proj((size_t)num_primitives); - - if (type == BVH_MODEL_TRIANGLES) { - for (unsigned int i = 0; i < num_primitives; ++i) { - const Triangle& t = tri_indices[primitive_indices[i]]; - proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + - vertices[t[2]][split_axis]) / - 3; - } - } else if (type == BVH_MODEL_POINTCLOUD) { - for (unsigned int i = 0; i < num_primitives; ++i) - proj[i] = vertices[primitive_indices[i]][split_axis]; - } - - std::sort(proj.begin(), proj.end()); - - if (num_primitives % 2 == 1) { - split_value = proj[(num_primitives - 1) / 2]; - } else { - split_value = - (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2; - } - } -}; - -template <> -bool HPP_FCL_DLLAPI BVSplitter::apply(const Vec3f& q) const; - -template <> -bool HPP_FCL_DLLAPI BVSplitter::apply(const Vec3f& q) const; - -template <> -bool HPP_FCL_DLLAPI BVSplitter::apply(const Vec3f& q) const; - -template <> -bool HPP_FCL_DLLAPI BVSplitter::apply(const Vec3f& q) const; - -template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_bvcenter( - const OBB& bv, unsigned int* primitive_indices, - unsigned int num_primitives); - -template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_mean( - const OBB& bv, unsigned int* primitive_indices, - unsigned int num_primitives); - -template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_median( - const OBB& bv, unsigned int* primitive_indices, - unsigned int num_primitives); - -template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_bvcenter( - const RSS& bv, unsigned int* primitive_indices, - unsigned int num_primitives); - -template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_mean( - const RSS& bv, unsigned int* primitive_indices, - unsigned int num_primitives); - -template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_median( - const RSS& bv, unsigned int* primitive_indices, - unsigned int num_primitives); - -template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_bvcenter( - const kIOS& bv, unsigned int* primitive_indices, - unsigned int num_primitives); - -template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_mean( - const kIOS& bv, unsigned int* primitive_indices, - unsigned int num_primitives); - -template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_median( - const kIOS& bv, unsigned int* primitive_indices, - unsigned int num_primitives); - -template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_bvcenter( - const OBBRSS& bv, unsigned int* primitive_indices, - unsigned int num_primitives); - -template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_mean( - const OBBRSS& bv, unsigned int* primitive_indices, - unsigned int num_primitives); - -template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_median( - const OBBRSS& bv, unsigned int* primitive_indices, - unsigned int num_primitives); - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/internal/intersect.h b/include/hpp/fcl/internal/intersect.h index 1166bc74f..bc4fe38c4 100644 --- a/include/hpp/fcl/internal/intersect.h +++ b/include/hpp/fcl/internal/intersect.h @@ -1,190 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_INTERSECT_H -#define HPP_FCL_INTERSECT_H - -/// @cond INTERNAL - -#include - -namespace hpp { -namespace fcl { - -/// @brief CCD intersect kernel among primitives -class HPP_FCL_DLLAPI Intersect { - public: - static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, - const Vec3f& v3, Vec3f* n, FCL_REAL* t); -}; // class Intersect - -/// @brief Project functions -class HPP_FCL_DLLAPI Project { - public: - struct HPP_FCL_DLLAPI ProjectResult { - /// @brief Parameterization of the projected point (based on the simplex to - /// be projected, use 2 or 3 or 4 of the array) - FCL_REAL parameterization[4]; - - /// @brief square distance from the query point to the projected simplex - FCL_REAL sqr_distance; - - /// @brief the code of the projection type - unsigned int encode; - - ProjectResult() : sqr_distance(-1), encode(0) {} - }; - - /// @brief Project point p onto line a-b - static ProjectResult projectLine(const Vec3f& a, const Vec3f& b, - const Vec3f& p); - - /// @brief Project point p onto triangle a-b-c - static ProjectResult projectTriangle(const Vec3f& a, const Vec3f& b, - const Vec3f& c, const Vec3f& p); - - /// @brief Project point p onto tetrahedra a-b-c-d - static ProjectResult projectTetrahedra(const Vec3f& a, const Vec3f& b, - const Vec3f& c, const Vec3f& d, - const Vec3f& p); - - /// @brief Project origin (0) onto line a-b - static ProjectResult projectLineOrigin(const Vec3f& a, const Vec3f& b); - - /// @brief Project origin (0) onto triangle a-b-c - static ProjectResult projectTriangleOrigin(const Vec3f& a, const Vec3f& b, - const Vec3f& c); - - /// @brief Project origin (0) onto tetrahedran a-b-c-d - static ProjectResult projectTetrahedraOrigin(const Vec3f& a, const Vec3f& b, - const Vec3f& c, const Vec3f& d); -}; - -/// @brief Triangle distance functions -class HPP_FCL_DLLAPI TriangleDistance { - public: - /// @brief Returns closest points between an segment pair. - /// The first segment is P + t * A - /// The second segment is Q + t * B - /// X, Y are the closest points on the two segments - /// VEC is the vector between X and Y - static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, - const Vec3f& B, Vec3f& VEC, Vec3f& X, Vec3f& Y); - - /// Compute squared distance between triangles - /// @param S and T are two triangles - /// @retval P, Q closest points if triangles do not intersect. - /// @return squared distance if triangles do not intersect, 0 otherwise. - /// If the triangles are disjoint, P and Q give the closet points of - /// S and T respectively. However, - /// if the triangles overlap, P and Q are basically a random pair of points - /// from the triangles, not coincident points on the intersection of the - /// triangles, as might be expected. - static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, - Vec3f& Q); - - static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, Vec3f& P, - Vec3f& Q); - - /// Compute squared distance between triangles - /// @param S and T are two triangles - /// @param R, Tl, rotation and translation applied to T, - /// @retval P, Q closest points if triangles do not intersect. - /// @return squared distance if triangles do not intersect, 0 otherwise. - /// If the triangles are disjoint, P and Q give the closet points of - /// S and T respectively. However, - /// if the triangles overlap, P and Q are basically a random pair of points - /// from the triangles, not coincident points on the intersection of the - /// triangles, as might be expected. - static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - const Matrix3f& R, const Vec3f& Tl, Vec3f& P, - Vec3f& Q); - - /// Compute squared distance between triangles - /// @param S and T are two triangles - /// @param tf, rotation and translation applied to T, - /// @retval P, Q closest points if triangles do not intersect. - /// @return squared distance if triangles do not intersect, 0 otherwise. - /// If the triangles are disjoint, P and Q give the closet points of - /// S and T respectively. However, - /// if the triangles overlap, P and Q are basically a random pair of points - /// from the triangles, not coincident points on the intersection of the - /// triangles, as might be expected. - static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - const Transform3f& tf, Vec3f& P, Vec3f& Q); - - /// Compute squared distance between triangles - /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices - /// @param R, Tl, rotation and translation applied to T1, T2, T3, - /// @retval P, Q closest points if triangles do not intersect. - /// @return squared distance if triangles do not intersect, 0 otherwise. - /// If the triangles are disjoint, P and Q give the closet points of - /// S and T respectively. However, - /// if the triangles overlap, P and Q are basically a random pair of points - /// from the triangles, not coincident points on the intersection of the - /// triangles, as might be expected. - static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - const Matrix3f& R, const Vec3f& Tl, Vec3f& P, - Vec3f& Q); - - /// Compute squared distance between triangles - /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices - /// @param tf, rotation and translation applied to T1, T2, T3, - /// @retval P, Q closest points if triangles do not intersect. - /// @return squared distance if triangles do not intersect, 0 otherwise. - /// If the triangles are disjoint, P and Q give the closet points of - /// S and T respectively. However, - /// if the triangles overlap, P and Q are basically a random pair of points - /// from the triangles, not coincident points on the intersection of the - /// triangles, as might be expected. - static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - const Transform3f& tf, Vec3f& P, Vec3f& Q); -}; - -} // namespace fcl - -} // namespace hpp - -/// @endcond - -#endif +#include +#include diff --git a/include/hpp/fcl/internal/shape_shape_contact_patch_func.h b/include/hpp/fcl/internal/shape_shape_contact_patch_func.h index 9e3f6ec01..cc3ecf58a 100644 --- a/include/hpp/fcl/internal/shape_shape_contact_patch_func.h +++ b/include/hpp/fcl/internal/shape_shape_contact_patch_func.h @@ -1,266 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2024, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Louis Montaut */ - -#ifndef HPP_FCL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H -#define HPP_FCL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H - -#include "hpp/fcl/collision_data.h" -#include "hpp/fcl/collision_utility.h" -#include "hpp/fcl/narrowphase/narrowphase.h" -#include "hpp/fcl/contact_patch/contact_patch_solver.h" -#include "hpp/fcl/shape/geometric_shapes_traits.h" - -namespace hpp { -namespace fcl { - -/// @brief Shape-shape contact patch computation. -/// Assumes that `csolver` and the `ContactPatchResult` have already been set up -/// by the `ContactPatchRequest`. -template -struct ComputeShapeShapeContactPatch { - static void run(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const CollisionResult& collision_result, - const ContactPatchSolver* csolver, - const ContactPatchRequest& request, - ContactPatchResult& result) { - // Note: see specializations for Plane and Halfspace below. - if (!collision_result.isCollision()) { - return; - } - HPP_FCL_ASSERT( - result.check(request), - "The contact patch result and request are incompatible (issue of " - "contact patch size or maximum number of contact patches). Make sure " - "result is initialized with request.", - std::logic_error); - - const ShapeType1& s1 = static_cast(*o1); - const ShapeType2& s2 = static_cast(*o2); - for (size_t i = 0; i < collision_result.numContacts(); ++i) { - if (i >= request.max_num_patch) { - break; - } - csolver->setSupportGuess(collision_result.cached_support_func_guess); - const Contact& contact = collision_result.getContact(i); - ContactPatch& contact_patch = result.getUnusedContactPatch(); - csolver->computePatch(s1, tf1, s2, tf2, contact, contact_patch); - } - } -}; - -/// @brief Computes the contact patch between a Plane/Halfspace and another -/// shape. -/// @tparam InvertShapes set to true if the first shape of the collision pair -/// is s2 and not s1 (if you had to invert (s1, tf1) and (s2, tf2) when calling -/// this function). -template -void computePatchPlaneOrHalfspace(const OtherShapeType& s1, - const Transform3f& tf1, - const PlaneOrHalfspace& s2, - const Transform3f& tf2, - const ContactPatchSolver* csolver, - const Contact& contact, - ContactPatch& contact_patch) { - HPP_FCL_UNUSED_VARIABLE(s2); - HPP_FCL_UNUSED_VARIABLE(tf2); - constructContactPatchFrameFromContact(contact, contact_patch); - if ((bool)(shape_traits::IsStrictlyConvex)) { - // Only one point of contact; it has already been computed. - contact_patch.addPoint(contact.pos); - return; - } - - // We only need to compute the support set in the direction of the normal. - // We need to temporarily express the patch in the local frame of shape1. - SupportSet& support_set = csolver->support_set_shape1; - support_set.tf.rotation().noalias() = - tf1.rotation().transpose() * contact_patch.tf.rotation(); - support_set.tf.translation().noalias() = - tf1.rotation().transpose() * - (contact_patch.tf.translation() - tf1.translation()); - - // Note: for now, taking into account swept-sphere radius does not change - // anything to the support set computations. However it will be used in the - // future if we want to store the offsets to the support plane for each point - // in a support set. - using SupportOptions = details::SupportOptions; - if (InvertShapes) { - support_set.direction = ContactPatch::PatchDirection::INVERTED; - details::getShapeSupportSet( - &s1, support_set, csolver->support_guess[1], csolver->supports_data[1], - csolver->num_samples_curved_shapes, csolver->patch_tolerance); - } else { - support_set.direction = ContactPatch::PatchDirection::DEFAULT; - details::getShapeSupportSet( - &s1, support_set, csolver->support_guess[0], csolver->supports_data[0], - csolver->num_samples_curved_shapes, csolver->patch_tolerance); - } - csolver->getResult(contact, &(support_set.points()), contact_patch); -} - -#define PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(PlaneOrHspace) \ - template \ - struct ComputeShapeShapeContactPatch { \ - static void run(const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ - const CollisionResult& collision_result, \ - const ContactPatchSolver* csolver, \ - const ContactPatchRequest& request, \ - ContactPatchResult& result) { \ - if (!collision_result.isCollision()) { \ - return; \ - } \ - HPP_FCL_ASSERT( \ - result.check(request), \ - "The contact patch result and request are incompatible (issue of " \ - "contact patch size or maximum number of contact patches). Make " \ - "sure " \ - "result is initialized with request.", \ - std::logic_error); \ - \ - const OtherShapeType& s1 = static_cast(*o1); \ - const PlaneOrHspace& s2 = static_cast(*o2); \ - for (size_t i = 0; i < collision_result.numContacts(); ++i) { \ - if (i >= request.max_num_patch) { \ - break; \ - } \ - csolver->setSupportGuess(collision_result.cached_support_func_guess); \ - const Contact& contact = collision_result.getContact(i); \ - ContactPatch& contact_patch = result.getUnusedContactPatch(); \ - computePatchPlaneOrHalfspace( \ - s1, tf1, s2, tf2, csolver, contact, contact_patch); \ - } \ - } \ - }; \ - \ - template \ - struct ComputeShapeShapeContactPatch { \ - static void run(const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ - const CollisionResult& collision_result, \ - const ContactPatchSolver* csolver, \ - const ContactPatchRequest& request, \ - ContactPatchResult& result) { \ - if (!collision_result.isCollision()) { \ - return; \ - } \ - HPP_FCL_ASSERT( \ - result.check(request), \ - "The contact patch result and request are incompatible (issue of " \ - "contact patch size or maximum number of contact patches). Make " \ - "sure " \ - "result is initialized with request.", \ - std::logic_error); \ - \ - const PlaneOrHspace& s1 = static_cast(*o1); \ - const OtherShapeType& s2 = static_cast(*o2); \ - for (size_t i = 0; i < collision_result.numContacts(); ++i) { \ - if (i >= request.max_num_patch) { \ - break; \ - } \ - csolver->setSupportGuess(collision_result.cached_support_func_guess); \ - const Contact& contact = collision_result.getContact(i); \ - ContactPatch& contact_patch = result.getUnusedContactPatch(); \ - computePatchPlaneOrHalfspace( \ - s2, tf2, s1, tf1, csolver, contact, contact_patch); \ - } \ - } \ - }; - -PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Plane) -PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Halfspace) - -#define PLANE_HSPACE_CONTACT_PATCH(PlaneOrHspace1, PlaneOrHspace2) \ - template <> \ - struct ComputeShapeShapeContactPatch { \ - static void run(const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ - const CollisionResult& collision_result, \ - const ContactPatchSolver* csolver, \ - const ContactPatchRequest& request, \ - ContactPatchResult& result) { \ - HPP_FCL_UNUSED_VARIABLE(o1); \ - HPP_FCL_UNUSED_VARIABLE(tf1); \ - HPP_FCL_UNUSED_VARIABLE(o2); \ - HPP_FCL_UNUSED_VARIABLE(tf2); \ - HPP_FCL_UNUSED_VARIABLE(csolver); \ - if (!collision_result.isCollision()) { \ - return; \ - } \ - HPP_FCL_ASSERT( \ - result.check(request), \ - "The contact patch result and request are incompatible (issue of " \ - "contact patch size or maximum number of contact patches). Make " \ - "sure " \ - "result is initialized with request.", \ - std::logic_error); \ - \ - for (size_t i = 0; i < collision_result.numContacts(); ++i) { \ - if (i >= request.max_num_patch) { \ - break; \ - } \ - const Contact& contact = collision_result.getContact(i); \ - ContactPatch& contact_patch = result.getUnusedContactPatch(); \ - constructContactPatchFrameFromContact(contact, contact_patch); \ - contact_patch.addPoint(contact.pos); \ - } \ - } \ - }; - -PLANE_HSPACE_CONTACT_PATCH(Plane, Plane) -PLANE_HSPACE_CONTACT_PATCH(Plane, Halfspace) -PLANE_HSPACE_CONTACT_PATCH(Halfspace, Plane) -PLANE_HSPACE_CONTACT_PATCH(Halfspace, Halfspace) - -#undef PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH -#undef PLANE_HSPACE_CONTACT_PATCH - -template -void ShapeShapeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const CollisionResult& collision_result, - const ContactPatchSolver* csolver, - const ContactPatchRequest& request, - ContactPatchResult& result) { - return ComputeShapeShapeContactPatch::run( - o1, tf1, o2, tf2, collision_result, csolver, request, result); -} - -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/internal/shape_shape_func.h b/include/hpp/fcl/internal/shape_shape_func.h index 3321b4b38..3789fb52a 100644 --- a/include/hpp/fcl/internal/shape_shape_func.h +++ b/include/hpp/fcl/internal/shape_shape_func.h @@ -1,316 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2014, CNRS-LAAS - * Copyright (c) 2024, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Florent Lamiraux */ - -#ifndef HPP_FCL_INTERNAL_SHAPE_SHAPE_FUNC_H -#define HPP_FCL_INTERNAL_SHAPE_SHAPE_FUNC_H - -/// @cond INTERNAL - -#include -#include -#include -#include - -namespace hpp { -namespace fcl { - -template -struct ShapeShapeDistancer { - static FCL_REAL run(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, const DistanceRequest& request, - DistanceResult& result) { - if (request.isSatisfied(result)) return result.min_distance; - - // Witness points on shape1 and shape2, normal pointing from shape1 to - // shape2. - Vec3f p1, p2, normal; - const FCL_REAL distance = ShapeShapeDistancer::run( - o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, p1, p2, - normal); - - result.update(distance, o1, o2, DistanceResult::NONE, DistanceResult::NONE, - p1, p2, normal); - - return distance; - } - - static FCL_REAL run(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const bool compute_signed_distance, Vec3f& p1, Vec3f& p2, - Vec3f& normal) { - const ShapeType1* obj1 = static_cast(o1); - const ShapeType2* obj2 = static_cast(o2); - return nsolver->shapeDistance(*obj1, tf1, *obj2, tf2, - compute_signed_distance, p1, p2, normal); - } -}; - -/// @brief Shape-shape distance computation. -/// Assumes that `nsolver` has already been set up by a `DistanceRequest` or -/// a `CollisionRequest`. -/// -/// @note This function is typically used for collision pairs containing two -/// primitive shapes. -/// @note This function might be specialized for some pairs of shapes. -template -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) { - return ShapeShapeDistancer::run( - o1, tf1, o2, tf2, nsolver, request, result); -} - -namespace internal { -/// @brief Shape-shape distance computation. -/// Assumes that `nsolver` has already been set up by a `DistanceRequest` or -/// a `CollisionRequest`. -/// -/// @note This function is typically used for collision pairs complex structures -/// like BVHs, HeightFields or Octrees. These structures contain sets of -/// primitive shapes. -/// This function is meant to be called on the pairs of primitive shapes of -/// these structures. -/// @note This function might be specialized for some pairs of shapes. -template -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const bool compute_signed_distance, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { - return ::hpp::fcl::ShapeShapeDistancer::run( - o1, tf1, o2, tf2, nsolver, compute_signed_distance, p1, p2, normal); -} -} // namespace internal - -/// @brief Shape-shape collision detection. -/// Assumes that `nsolver` has already been set up by a `DistanceRequest` or -/// a `CollisionRequest`. -/// -/// @note This function is typically used for collision pairs containing two -/// primitive shapes. -/// Complex structures like BVHs, HeightFields or Octrees contain sets of -/// primitive shapes should use the `ShapeShapeDistance` function to do their -/// internal collision detection checks. -template -struct ShapeShapeCollider { - static std::size_t run(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - if (request.isSatisfied(result)) return result.numContacts(); - - const bool compute_penetration = - request.enable_contact || (request.security_margin < 0); - Vec3f p1, p2, normal; - FCL_REAL distance = internal::ShapeShapeDistance( - o1, tf1, o2, tf2, nsolver, compute_penetration, p1, p2, normal); - - size_t num_contacts = 0; - const FCL_REAL distToCollision = distance - request.security_margin; - - internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision, - p1, p2, normal); - if (distToCollision <= request.collision_distance_threshold && - result.numContacts() < request.num_max_contacts) { - if (result.numContacts() < request.num_max_contacts) { - Contact contact(o1, o2, Contact::NONE, Contact::NONE, p1, p2, normal, - distance); - result.addContact(contact); - } - num_contacts = result.numContacts(); - } - - return num_contacts; - } -}; - -template -std::size_t ShapeShapeCollide(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - return ShapeShapeCollider::run( - o1, tf1, o2, tf2, nsolver, request, result); -} - -// clang-format off -// ============================================================================================================== -// ============================================================================================================== -// ============================================================================================================== -// Shape distance algorithms based on: -// - built-in function: 0 -// - GJK: 1 -// -// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ -// | | box | sphere | capsule | cone | cylinder | plane | half-space | triangle | ellipsoid | convex | -// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ -// | box | 1 | 0 | 1 | 1 | 1 | 0 | 0 | 1 | 1 | 1 | -// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ -// | sphere |/////| 0 | 0 | 1 | 0 | 0 | 0 | 0 | 1 | 1 | -// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ -// | capsule |/////|////////| 0 | 1 | 1 | 0 | 0 | 1 | 1 | 1 | -// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ -// | cone |/////|////////|/////////| 1 | 1 | 0 | 0 | 1 | 1 | 1 | -// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ -// | cylinder |/////|////////|/////////|//////| 1 | 0 | 0 | 1 | 1 | 1 | -// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ -// | plane |/////|////////|/////////|//////|//////////| ? | ? | 0 | 0 | 0 | -// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ -// | half-space |/////|////////|/////////|//////|//////////|///////| ? | 0 | 0 | 0 | -// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ -// | triangle |/////|////////|/////////|//////|//////////|///////|////////////| 0 | 1 | 1 | -// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ -// | ellipsoid |/////|////////|/////////|//////|//////////|///////|////////////|//////////| 1 | 1 | -// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ -// | convex |/////|////////|/////////|//////|//////////|///////|////////////|//////////|///////////| 1 | -// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ -// -// Number of pairs: 55 -// - Specialized: 26 -// - GJK: 29 -// clang-format on - -#define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2) \ - template <> \ - HPP_FCL_DLLAPI FCL_REAL internal::ShapeShapeDistance( \ - const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ - const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \ - Vec3f& p2, Vec3f& normal); \ - template <> \ - HPP_FCL_DLLAPI FCL_REAL internal::ShapeShapeDistance( \ - const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ - const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \ - Vec3f& p2, Vec3f& normal); \ - template <> \ - inline HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance( \ - const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ - const GJKSolver* nsolver, const DistanceRequest& request, \ - DistanceResult& result) { \ - result.o1 = o1; \ - result.o2 = o2; \ - result.b1 = DistanceResult::NONE; \ - result.b2 = DistanceResult::NONE; \ - result.min_distance = internal::ShapeShapeDistance( \ - o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \ - result.nearest_points[0], result.nearest_points[1], result.normal); \ - return result.min_distance; \ - } \ - template <> \ - inline HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance( \ - const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ - const GJKSolver* nsolver, const DistanceRequest& request, \ - DistanceResult& result) { \ - result.o1 = o1; \ - result.o2 = o2; \ - result.b1 = DistanceResult::NONE; \ - result.b2 = DistanceResult::NONE; \ - result.min_distance = internal::ShapeShapeDistance( \ - o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \ - result.nearest_points[0], result.nearest_points[1], result.normal); \ - return result.min_distance; \ - } - -#define SHAPE_SELF_DISTANCE_SPECIALIZATION(T) \ - template <> \ - HPP_FCL_DLLAPI FCL_REAL internal::ShapeShapeDistance( \ - const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ - const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \ - Vec3f& p2, Vec3f& normal); \ - template <> \ - inline HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance( \ - const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ - const GJKSolver* nsolver, const DistanceRequest& request, \ - DistanceResult& result) { \ - result.o1 = o1; \ - result.o2 = o2; \ - result.b1 = DistanceResult::NONE; \ - result.b2 = DistanceResult::NONE; \ - result.min_distance = internal::ShapeShapeDistance( \ - o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \ - result.nearest_points[0], result.nearest_points[1], result.normal); \ - return result.min_distance; \ - } - -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Halfspace) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Plane) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Sphere) -SHAPE_SELF_DISTANCE_SPECIALIZATION(Capsule) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Halfspace) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Plane) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Halfspace) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Plane) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Halfspace) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Plane) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Halfspace) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Plane) -SHAPE_SELF_DISTANCE_SPECIALIZATION(Sphere) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Halfspace) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Plane) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Halfspace) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Plane) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Plane) -SHAPE_SELF_DISTANCE_SPECIALIZATION(TriangleP) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Sphere) -SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Plane, Halfspace) -SHAPE_SELF_DISTANCE_SPECIALIZATION(Plane) -SHAPE_SELF_DISTANCE_SPECIALIZATION(Halfspace) - -#undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION -#undef SHAPE_SELF_DISTANCE_SPECIALIZATION - -} // namespace fcl -} // namespace hpp - -/// @endcond - -#endif +#include +#include diff --git a/include/hpp/fcl/internal/tools.h b/include/hpp/fcl/internal/tools.h index 346141a50..aa11afec1 100644 --- a/include/hpp/fcl/internal/tools.h +++ b/include/hpp/fcl/internal/tools.h @@ -1,215 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Joseph Mirabel */ - -#ifndef HPP_FCL_INTERNAL_TOOLS_H -#define HPP_FCL_INTERNAL_TOOLS_H - -#include - -#include -#include -#include - -#include - -namespace hpp { -namespace fcl { - -template -static inline typename Derived::Scalar triple( - const Eigen::MatrixBase& x, const Eigen::MatrixBase& y, - const Eigen::MatrixBase& z) { - return x.derived().dot(y.derived().cross(z.derived())); -} - -template -void generateCoordinateSystem(const Eigen::MatrixBase& _w, - const Eigen::MatrixBase& _u, - const Eigen::MatrixBase& _v) { - typedef typename Derived1::Scalar T; - - Eigen::MatrixBase& w = const_cast&>(_w); - Eigen::MatrixBase& u = const_cast&>(_u); - Eigen::MatrixBase& v = const_cast&>(_v); - - T inv_length; - if (std::abs(w[0]) >= std::abs(w[1])) { - inv_length = (T)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]); - u[0] = -w[2] * inv_length; - u[1] = (T)0; - u[2] = w[0] * inv_length; - v[0] = w[1] * u[2]; - v[1] = w[2] * u[0] - w[0] * u[2]; - v[2] = -w[1] * u[0]; - } else { - inv_length = (T)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]); - u[0] = (T)0; - u[1] = w[2] * inv_length; - u[2] = -w[1] * inv_length; - v[0] = w[1] * u[2] - w[2] * u[1]; - v[1] = -w[0] * u[2]; - v[2] = w[0] * u[1]; - } -} - -/* ----- Start Matrices ------ */ -template -void relativeTransform(const Eigen::MatrixBase& R1, - const Eigen::MatrixBase& t1, - const Eigen::MatrixBase& R2, - const Eigen::MatrixBase& t2, - const Eigen::MatrixBase& R, - const Eigen::MatrixBase& t) { - const_cast&>(R) = R1.transpose() * R2; - const_cast&>(t) = R1.transpose() * (t2 - t1); -} - -/// @brief compute the eigen vector and eigen vector of a matrix. dout is the -/// eigen values, vout is the eigen vectors -template -void eigen(const Eigen::MatrixBase& m, - typename Derived::Scalar dout[3], Vector* vout) { - typedef typename Derived::Scalar Scalar; - Derived R(m.derived()); - int n = 3; - int j, iq, ip, i; - Scalar tresh, theta, tau, t, sm, s, h, g, c; - - Scalar b[3]; - Scalar z[3]; - Scalar v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; - Scalar d[3]; - - for (ip = 0; ip < n; ++ip) { - b[ip] = d[ip] = R(ip, ip); - z[ip] = 0; - } - - for (i = 0; i < 50; ++i) { - sm = 0; - for (ip = 0; ip < n; ++ip) - for (iq = ip + 1; iq < n; ++iq) sm += std::abs(R(ip, iq)); - if (sm == 0.0) { - vout[0] << v[0][0], v[0][1], v[0][2]; - vout[1] << v[1][0], v[1][1], v[1][2]; - vout[2] << v[2][0], v[2][1], v[2][2]; - dout[0] = d[0]; - dout[1] = d[1]; - dout[2] = d[2]; - return; - } - - if (i < 3) - tresh = 0.2 * sm / (n * n); - else - tresh = 0.0; - - for (ip = 0; ip < n; ++ip) { - for (iq = ip + 1; iq < n; ++iq) { - g = 100.0 * std::abs(R(ip, iq)); - if (i > 3 && std::abs(d[ip]) + g == std::abs(d[ip]) && - std::abs(d[iq]) + g == std::abs(d[iq])) - R(ip, iq) = 0.0; - else if (std::abs(R(ip, iq)) > tresh) { - h = d[iq] - d[ip]; - if (std::abs(h) + g == std::abs(h)) - t = (R(ip, iq)) / h; - else { - theta = 0.5 * h / (R(ip, iq)); - t = 1.0 / (std::abs(theta) + std::sqrt(1.0 + theta * theta)); - if (theta < 0.0) t = -t; - } - c = 1.0 / std::sqrt(1 + t * t); - s = t * c; - tau = s / (1.0 + c); - h = t * R(ip, iq); - z[ip] -= h; - z[iq] += h; - d[ip] -= h; - d[iq] += h; - R(ip, iq) = 0.0; - for (j = 0; j < ip; ++j) { - g = R(j, ip); - h = R(j, iq); - R(j, ip) = g - s * (h + g * tau); - R(j, iq) = h + s * (g - h * tau); - } - for (j = ip + 1; j < iq; ++j) { - g = R(ip, j); - h = R(j, iq); - R(ip, j) = g - s * (h + g * tau); - R(j, iq) = h + s * (g - h * tau); - } - for (j = iq + 1; j < n; ++j) { - g = R(ip, j); - h = R(iq, j); - R(ip, j) = g - s * (h + g * tau); - R(iq, j) = h + s * (g - h * tau); - } - for (j = 0; j < n; ++j) { - g = v[j][ip]; - h = v[j][iq]; - v[j][ip] = g - s * (h + g * tau); - v[j][iq] = h + s * (g - h * tau); - } - } - } - } - for (ip = 0; ip < n; ++ip) { - b[ip] += z[ip]; - d[ip] = b[ip]; - z[ip] = 0.0; - } - } - - std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl; - - return; -} - -template -bool isEqual(const Eigen::MatrixBase& lhs, - const Eigen::MatrixBase& rhs, - const FCL_REAL tol = std::numeric_limits::epsilon() * - 100) { - return ((lhs - rhs).array().abs() < tol).all(); -} - -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/internal/traversal.h b/include/hpp/fcl/internal/traversal.h index e999d8c43..f0cbf51fc 100644 --- a/include/hpp/fcl/internal/traversal.h +++ b/include/hpp/fcl/internal/traversal.h @@ -1,76 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, LAAS CNRS - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Joseph Mirabel */ - -#ifndef HPP_FCL_TRAVERSAL_DETAILS_TRAVERSAL_H -#define HPP_FCL_TRAVERSAL_DETAILS_TRAVERSAL_H - -/// @cond INTERNAL - -namespace hpp { -namespace fcl { - -enum { RelativeTransformationIsIdentity = 1 }; - -namespace details { -template -struct HPP_FCL_DLLAPI RelativeTransformation { - RelativeTransformation() : R(Matrix3f::Identity()) {} - - const Matrix3f& _R() const { return R; } - const Vec3f& _T() const { return T; } - - Matrix3f R; - Vec3f T; -}; - -template <> -struct HPP_FCL_DLLAPI RelativeTransformation { - static const Matrix3f& _R() { - HPP_FCL_THROW_PRETTY("should never reach this point", std::logic_error); - } - static const Vec3f& _T() { - HPP_FCL_THROW_PRETTY("should never reach this point", std::logic_error); - } -}; -} // namespace details - -} // namespace fcl - -} // namespace hpp - -/// @endcond - -#endif +#include +#include diff --git a/include/hpp/fcl/internal/traversal_node_base.h b/include/hpp/fcl/internal/traversal_node_base.h index a3c56f8d4..b5f5d9938 100644 --- a/include/hpp/fcl/internal/traversal_node_base.h +++ b/include/hpp/fcl/internal/traversal_node_base.h @@ -1,175 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_TRAVERSAL_NODE_BASE_H -#define HPP_FCL_TRAVERSAL_NODE_BASE_H - -/// @cond INTERNAL - -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// @brief Node structure encoding the information required for traversal. - -class TraversalNodeBase { - public: - TraversalNodeBase() : enable_statistics(false) {} - - virtual ~TraversalNodeBase() {} - - virtual void preprocess() {} - - virtual void postprocess() {} - - /// @brief Whether b is a leaf node in the first BVH tree - virtual bool isFirstNodeLeaf(unsigned int /*b*/) const { return true; } - - /// @brief Whether b is a leaf node in the second BVH tree - virtual bool isSecondNodeLeaf(unsigned int /*b*/) const { return true; } - - /// @brief Traverse the subtree of the node in the first tree first - virtual bool firstOverSecond(unsigned int /*b1*/, unsigned int /*b2*/) const { - return true; - } - - /// @brief Get the left child of the node b in the first tree - virtual int getFirstLeftChild(unsigned int b) const { return (int)b; } - - /// @brief Get the right child of the node b in the first tree - virtual int getFirstRightChild(unsigned int b) const { return (int)b; } - - /// @brief Get the left child of the node b in the second tree - virtual int getSecondLeftChild(unsigned int b) const { return (int)b; } - - /// @brief Get the right child of the node b in the second tree - virtual int getSecondRightChild(unsigned int b) const { return (int)b; } - - /// @brief Whether store some statistics information during traversal - void enableStatistics(bool enable) { enable_statistics = enable; } - - /// @brief configuation of first object - Transform3f tf1; - - /// @brief configuration of second object - Transform3f tf2; - - /// @brief Whether stores statistics - bool enable_statistics; -}; - -/// @defgroup Traversal_For_Collision -/// regroup class about traversal for distance. -/// @{ - -/// @brief Node structure encoding the information required for collision -/// traversal. -class CollisionTraversalNodeBase : public TraversalNodeBase { - public: - CollisionTraversalNodeBase(const CollisionRequest& request_) - : request(request_), result(NULL) {} - - virtual ~CollisionTraversalNodeBase() {} - - /// BV test between b1 and b2 - /// @param b1, b2 Bounding volumes to test, - /// @retval sqrDistLowerBound square of a lower bound of the minimal - /// distance between bounding volumes. - virtual bool BVDisjoints(unsigned int b1, unsigned int b2, - FCL_REAL& sqrDistLowerBound) const = 0; - - /// @brief Leaf test between node b1 and b2, if they are both leafs - virtual void leafCollides(unsigned int /*b1*/, unsigned int /*b2*/, - FCL_REAL& /*sqrDistLowerBound*/) const = 0; - - /// @brief Check whether the traversal can stop - bool canStop() const { return this->request.isSatisfied(*(this->result)); } - - /// @brief request setting for collision - const CollisionRequest& request; - - /// @brief collision result kept during the traversal iteration - CollisionResult* result; -}; - -/// @} - -/// @defgroup Traversal_For_Distance -/// regroup class about traversal for distance. -/// @{ - -/// @brief Node structure encoding the information required for distance -/// traversal. -class DistanceTraversalNodeBase : public TraversalNodeBase { - public: - DistanceTraversalNodeBase() : result(NULL) {} - - virtual ~DistanceTraversalNodeBase() {} - - /// @brief BV test between b1 and b2 - /// @return a lower bound of the distance between the two BV. - /// @note except for OBB, this method returns the distance. - virtual FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, - unsigned int /*b2*/) const { - return (std::numeric_limits::max)(); - } - - /// @brief Leaf test between node b1 and b2, if they are both leafs - virtual void leafComputeDistance(unsigned int b1, unsigned int b2) const = 0; - - /// @brief Check whether the traversal can stop - virtual bool canStop(FCL_REAL /*c*/) const { return false; } - - /// @brief request setting for distance - DistanceRequest request; - - /// @brief distance result kept during the traversal iteration - DistanceResult* result; -}; - -///@} - -} // namespace fcl - -} // namespace hpp - -/// @endcond - -#endif +#include +#include diff --git a/include/hpp/fcl/internal/traversal_node_bvh_shape.h b/include/hpp/fcl/internal/traversal_node_bvh_shape.h index c24a0c002..c99f06b17 100644 --- a/include/hpp/fcl/internal/traversal_node_bvh_shape.h +++ b/include/hpp/fcl/internal/traversal_node_bvh_shape.h @@ -1,487 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_TRAVERSAL_NODE_MESH_SHAPE_H -#define HPP_FCL_TRAVERSAL_NODE_MESH_SHAPE_H - -/// @cond INTERNAL - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// @addtogroup Traversal_For_Collision -/// @{ - -/// @brief Traversal node for collision between BVH and shape -template -class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase { - public: - BVHShapeCollisionTraversalNode(const CollisionRequest& request) - : CollisionTraversalNodeBase(request) { - model1 = NULL; - model2 = NULL; - - num_bv_tests = 0; - num_leaf_tests = 0; - query_time_seconds = 0.0; - } - - /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(unsigned int b) const { - return model1->getBV(b).isLeaf(); - } - - /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(unsigned int b) const { - return model1->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(unsigned int b) const { - return model1->getBV(b).rightChild(); - } - - const BVHModel* model1; - const S* model2; - BV model2_bv; - - mutable int num_bv_tests; - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; -}; - -/// @brief Traversal node for collision between mesh and shape -template -class MeshShapeCollisionTraversalNode - : public BVHShapeCollisionTraversalNode { - public: - enum { - Options = _Options, - RTIsIdentity = _Options & RelativeTransformationIsIdentity - }; - - MeshShapeCollisionTraversalNode(const CollisionRequest& request) - : BVHShapeCollisionTraversalNode(request) { - vertices = NULL; - tri_indices = NULL; - - nsolver = NULL; - } - - /// test between BV b1 and shape - /// @param b1 BV to test, - /// @retval sqrDistLowerBound square of a lower bound of the minimal - /// distance between bounding volumes. - /// @brief BV culling test in one BVTT node - bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, - FCL_REAL& sqrDistLowerBound) const { - if (this->enable_statistics) this->num_bv_tests++; - bool disjoint; - if (RTIsIdentity) - disjoint = !this->model1->getBV(b1).bv.overlap( - this->model2_bv, this->request, sqrDistLowerBound); - else - disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), - this->model1->getBV(b1).bv, this->model2_bv, - this->request, sqrDistLowerBound); - if (disjoint) - internal::updateDistanceLowerBoundFromBV(this->request, *this->result, - sqrDistLowerBound); - assert(!disjoint || sqrDistLowerBound > 0); - return disjoint; - } - - /// @brief Intersection testing between leaves (one triangle and one shape) - void leafCollides(unsigned int b1, unsigned int /*b2*/, - FCL_REAL& sqrDistLowerBound) const { - if (this->enable_statistics) this->num_leaf_tests++; - const BVNode& node = this->model1->getBV(b1); - - int primitive_id = node.primitiveId(); - - const Triangle& tri_id = this->tri_indices[primitive_id]; - const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]], - this->vertices[tri_id[2]]); - - // When reaching this point, `this->solver` has already been set up - // by the CollisionRequest `this->request`. - // The only thing we need to (and can) pass to `ShapeShapeDistance` is - // whether or not penetration information is should be computed in case of - // collision. - const bool compute_penetration = - this->request.enable_contact || (this->request.security_margin < 0); - Vec3f c1, c2, normal; - FCL_REAL distance; - - if (RTIsIdentity) { - static const Transform3f Id; - distance = internal::ShapeShapeDistance( - &tri, Id, this->model2, this->tf2, this->nsolver, compute_penetration, - c1, c2, normal); - } else { - distance = internal::ShapeShapeDistance( - &tri, this->tf1, this->model2, this->tf2, this->nsolver, - compute_penetration, c1, c2, normal); - } - const FCL_REAL distToCollision = distance - this->request.security_margin; - - internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result), - distToCollision, c1, c2, normal); - - if (distToCollision <= this->request.collision_distance_threshold) { - sqrDistLowerBound = 0; - if (this->result->numContacts() < this->request.num_max_contacts) { - this->result->addContact(Contact(this->model1, this->model2, - primitive_id, Contact::NONE, c1, c2, - normal, distance)); - assert(this->result->isCollision()); - } - } else { - sqrDistLowerBound = distToCollision * distToCollision; - } - - assert(this->result->isCollision() || sqrDistLowerBound > 0); - } // leafCollides - - Vec3f* vertices; - Triangle* tri_indices; - - const GJKSolver* nsolver; -}; - -/// @} - -/// @addtogroup Traversal_For_Distance -/// @{ - -/// @brief Traversal node for distance computation between BVH and shape -template -class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase { - public: - BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { - model1 = NULL; - model2 = NULL; - - num_bv_tests = 0; - num_leaf_tests = 0; - query_time_seconds = 0.0; - } - - /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(unsigned int b) const { - return model1->getBV(b).isLeaf(); - } - - /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(unsigned int b) const { - return model1->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(unsigned int b) const { - return model1->getBV(b).rightChild(); - } - - /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { - return model1->getBV(b1).bv.distance(model2_bv); - } - - const BVHModel* model1; - const S* model2; - BV model2_bv; - - mutable int num_bv_tests; - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; -}; - -/// @brief Traversal node for distance computation between shape and BVH -template -class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase { - public: - ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase() { - model1 = NULL; - model2 = NULL; - - num_bv_tests = 0; - num_leaf_tests = 0; - query_time_seconds = 0.0; - } - - /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(unsigned int b) const { - return model2->getBV(b).isLeaf(); - } - - /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(unsigned int b) const { - return model2->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(unsigned int b) const { - return model2->getBV(b).rightChild(); - } - - /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const { - return model1_bv.distance(model2->getBV(b2).bv); - } - - const S* model1; - const BVHModel* model2; - BV model1_bv; - - mutable int num_bv_tests; - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; -}; - -/// @brief Traversal node for distance between mesh and shape -template -class MeshShapeDistanceTraversalNode - : public BVHShapeDistanceTraversalNode { - public: - MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode() { - vertices = NULL; - tri_indices = NULL; - - rel_err = 0; - abs_err = 0; - - nsolver = NULL; - } - - /// @brief Distance testing between leaves (one triangle and one shape) - void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const { - if (this->enable_statistics) this->num_leaf_tests++; - - const BVNode& node = this->model1->getBV(b1); - - int primitive_id = node.primitiveId(); - - const Triangle& tri_id = tri_indices[primitive_id]; - const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]], - this->vertices[tri_id[2]]); - - Vec3f p1, p2, normal; - const FCL_REAL distance = internal::ShapeShapeDistance( - &tri, this->tf1, this->model2, this->tf2, this->nsolver, - this->request.enable_signed_distance, p1, p2, normal); - - this->result->update(distance, this->model1, this->model2, primitive_id, - DistanceResult::NONE, p1, p2, normal); - } - - /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const { - if ((c >= this->result->min_distance - abs_err) && - (c * (1 + rel_err) >= this->result->min_distance)) - return true; - return false; - } - - Vec3f* vertices; - Triangle* tri_indices; - - FCL_REAL rel_err; - FCL_REAL abs_err; - - const GJKSolver* nsolver; -}; - -/// @cond IGNORE -namespace details { - -template -void meshShapeDistanceOrientedNodeleafComputeDistance( - unsigned int b1, unsigned int /* b2 */, const BVHModel* model1, - const S& model2, Vec3f* vertices, Triangle* tri_indices, - const Transform3f& tf1, const Transform3f& tf2, const GJKSolver* nsolver, - bool enable_statistics, int& num_leaf_tests, const DistanceRequest& request, - DistanceResult& result) { - if (enable_statistics) num_leaf_tests++; - - const BVNode& node = model1->getBV(b1); - int primitive_id = node.primitiveId(); - - const Triangle& tri_id = tri_indices[primitive_id]; - const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]], - vertices[tri_id[2]]); - - Vec3f p1, p2, normal; - const FCL_REAL distance = internal::ShapeShapeDistance( - &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2, - normal); - - result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE, - p1, p2, normal); -} - -template -static inline void distancePreprocessOrientedNode( - const BVHModel* model1, Vec3f* vertices, Triangle* tri_indices, - int init_tri_id, const S& model2, const Transform3f& tf1, - const Transform3f& tf2, const GJKSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) { - const Triangle& tri_id = tri_indices[init_tri_id]; - const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]], - vertices[tri_id[2]]); - - Vec3f p1, p2, normal; - const FCL_REAL distance = internal::ShapeShapeDistance( - &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2, - normal); - result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE, - p1, p2, normal); -} - -} // namespace details - -/// @endcond - -/// @brief Traversal node for distance between mesh and shape, when mesh BVH is -/// one of the oriented node (RSS, kIOS, OBBRSS) -template -class MeshShapeDistanceTraversalNodeRSS - : public MeshShapeDistanceTraversalNode { - public: - MeshShapeDistanceTraversalNodeRSS() - : MeshShapeDistanceTraversalNode() {} - - void preprocess() { - details::distancePreprocessOrientedNode( - this->model1, this->vertices, this->tri_indices, 0, *(this->model2), - this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); - } - - void postprocess() {} - - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { - if (this->enable_statistics) this->num_bv_tests++; - return distance(this->tf1.getRotation(), this->tf1.getTranslation(), - this->model2_bv, this->model1->getBV(b1).bv); - } - - void leafComputeDistance(unsigned int b1, unsigned int b2) const { - details::meshShapeDistanceOrientedNodeleafComputeDistance( - b1, b2, this->model1, *(this->model2), this->vertices, - this->tri_indices, this->tf1, this->tf2, this->nsolver, - this->enable_statistics, this->num_leaf_tests, this->request, - *(this->result)); - } -}; - -template -class MeshShapeDistanceTraversalNodekIOS - : public MeshShapeDistanceTraversalNode { - public: - MeshShapeDistanceTraversalNodekIOS() - : MeshShapeDistanceTraversalNode() {} - - void preprocess() { - details::distancePreprocessOrientedNode( - this->model1, this->vertices, this->tri_indices, 0, *(this->model2), - this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); - } - - void postprocess() {} - - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { - if (this->enable_statistics) this->num_bv_tests++; - return distance(this->tf1.getRotation(), this->tf1.getTranslation(), - this->model2_bv, this->model1->getBV(b1).bv); - } - - void leafComputeDistance(unsigned int b1, unsigned int b2) const { - details::meshShapeDistanceOrientedNodeleafComputeDistance( - b1, b2, this->model1, *(this->model2), this->vertices, - this->tri_indices, this->tf1, this->tf2, this->nsolver, - this->enable_statistics, this->num_leaf_tests, this->request, - *(this->result)); - } -}; - -template -class MeshShapeDistanceTraversalNodeOBBRSS - : public MeshShapeDistanceTraversalNode { - public: - MeshShapeDistanceTraversalNodeOBBRSS() - : MeshShapeDistanceTraversalNode() {} - - void preprocess() { - details::distancePreprocessOrientedNode( - this->model1, this->vertices, this->tri_indices, 0, *(this->model2), - this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); - } - - void postprocess() {} - - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { - if (this->enable_statistics) this->num_bv_tests++; - return distance(this->tf1.getRotation(), this->tf1.getTranslation(), - this->model2_bv, this->model1->getBV(b1).bv); - } - - void leafComputeDistance(unsigned int b1, unsigned int b2) const { - details::meshShapeDistanceOrientedNodeleafComputeDistance( - b1, b2, this->model1, *(this->model2), this->vertices, - this->tri_indices, this->tf1, this->tf2, this->nsolver, - this->enable_statistics, this->num_leaf_tests, this->request, - *(this->result)); - } -}; - -/// @} - -} // namespace fcl -} // namespace hpp - -/// @endcond - -#endif +#include +#include diff --git a/include/hpp/fcl/internal/traversal_node_bvhs.h b/include/hpp/fcl/internal/traversal_node_bvhs.h index bd20af2ac..835610130 100644 --- a/include/hpp/fcl/internal/traversal_node_bvhs.h +++ b/include/hpp/fcl/internal/traversal_node_bvhs.h @@ -1,557 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_TRAVERSAL_NODE_MESHES_H -#define HPP_FCL_TRAVERSAL_NODE_MESHES_H - -/// @cond INTERNAL - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace hpp { -namespace fcl { - -/// @addtogroup Traversal_For_Collision -/// @{ - -/// @brief Traversal node for collision between BVH models -template -class BVHCollisionTraversalNode : public CollisionTraversalNodeBase { - public: - BVHCollisionTraversalNode(const CollisionRequest& request) - : CollisionTraversalNodeBase(request) { - model1 = NULL; - model2 = NULL; - - num_bv_tests = 0; - num_leaf_tests = 0; - query_time_seconds = 0.0; - } - - /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(unsigned int b) const { - assert(model1 != NULL && "model1 is NULL"); - return model1->getBV(b).isLeaf(); - } - - /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(unsigned int b) const { - assert(model2 != NULL && "model2 is NULL"); - return model2->getBV(b).isLeaf(); - } - - /// @brief Determine the traversal order, is the first BVTT subtree better - bool firstOverSecond(unsigned int b1, unsigned int b2) const { - FCL_REAL sz1 = model1->getBV(b1).bv.size(); - FCL_REAL sz2 = model2->getBV(b2).bv.size(); - - bool l1 = model1->getBV(b1).isLeaf(); - bool l2 = model2->getBV(b2).isLeaf(); - - if (l2 || (!l1 && (sz1 > sz2))) return true; - return false; - } - - /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(unsigned int b) const { - return model1->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(unsigned int b) const { - return model1->getBV(b).rightChild(); - } - - /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(unsigned int b) const { - return model2->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(unsigned int b) const { - return model2->getBV(b).rightChild(); - } - - /// @brief The first BVH model - const BVHModel* model1; - /// @brief The second BVH model - const BVHModel* model2; - - /// @brief statistical information - mutable int num_bv_tests; - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; -}; - -/// @brief Traversal node for collision between two meshes -template -class MeshCollisionTraversalNode : public BVHCollisionTraversalNode { - public: - enum { - Options = _Options, - RTIsIdentity = _Options & RelativeTransformationIsIdentity - }; - - MeshCollisionTraversalNode(const CollisionRequest& request) - : BVHCollisionTraversalNode(request) { - vertices1 = NULL; - vertices2 = NULL; - tri_indices1 = NULL; - tri_indices2 = NULL; - } - - /// BV test between b1 and b2 - /// @param b1, b2 Bounding volumes to test, - /// @retval sqrDistLowerBound square of a lower bound of the minimal - /// distance between bounding volumes. - bool BVDisjoints(unsigned int b1, unsigned int b2, - FCL_REAL& sqrDistLowerBound) const { - if (this->enable_statistics) this->num_bv_tests++; - bool disjoint; - if (RTIsIdentity) - disjoint = !this->model1->getBV(b1).overlap( - this->model2->getBV(b2), this->request, sqrDistLowerBound); - else { - disjoint = !overlap(RT._R(), RT._T(), this->model2->getBV(b2).bv, - this->model1->getBV(b1).bv, this->request, - sqrDistLowerBound); - } - if (disjoint) - internal::updateDistanceLowerBoundFromBV(this->request, *this->result, - sqrDistLowerBound); - return disjoint; - } - - /// Intersection testing between leaves (two triangles) - /// - /// @param b1, b2 id of primitive in bounding volume hierarchy - /// @retval sqrDistLowerBound squared lower bound of distance between - /// primitives if they are not in collision. - /// - /// This method supports a security margin. If the distance between - /// the primitives is less than the security margin, the objects are - /// considered as in collision. in this case a contact point is - /// returned in the CollisionResult. - /// - /// @note If the distance between objects is less than the security margin, - /// and the object are not colliding, the penetration depth is - /// negative. - void leafCollides(unsigned int b1, unsigned int b2, - FCL_REAL& sqrDistLowerBound) const { - if (this->enable_statistics) this->num_leaf_tests++; - - const BVNode& node1 = this->model1->getBV(b1); - const BVNode& node2 = this->model2->getBV(b2); - - int primitive_id1 = node1.primitiveId(); - int primitive_id2 = node2.primitiveId(); - - const Triangle& tri_id1 = tri_indices1[primitive_id1]; - const Triangle& tri_id2 = tri_indices2[primitive_id2]; - - const Vec3f& P1 = vertices1[tri_id1[0]]; - const Vec3f& P2 = vertices1[tri_id1[1]]; - const Vec3f& P3 = vertices1[tri_id1[2]]; - const Vec3f& Q1 = vertices2[tri_id2[0]]; - const Vec3f& Q2 = vertices2[tri_id2[1]]; - const Vec3f& Q3 = vertices2[tri_id2[2]]; - - TriangleP tri1(P1, P2, P3); - TriangleP tri2(Q1, Q2, Q3); - - // TODO(louis): MeshCollisionTraversalNode should have its own GJKSolver. - GJKSolver solver(this->request); - - const bool compute_penetration = - this->request.enable_contact || (this->request.security_margin < 0); - Vec3f p1, p2, normal; - DistanceResult distanceResult; - FCL_REAL distance = internal::ShapeShapeDistance( - &tri1, this->tf1, &tri2, this->tf2, &solver, compute_penetration, p1, - p2, normal); - - const FCL_REAL distToCollision = distance - this->request.security_margin; - - internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result), - distToCollision, p1, p2, normal); - - if (distToCollision <= - this->request.collision_distance_threshold) { // collision - sqrDistLowerBound = 0; - if (this->result->numContacts() < this->request.num_max_contacts) { - this->result->addContact(Contact(this->model1, this->model2, - primitive_id1, primitive_id2, p1, p2, - normal, distance)); - } - } else - sqrDistLowerBound = distToCollision * distToCollision; - } - - Vec3f* vertices1; - Vec3f* vertices2; - - Triangle* tri_indices1; - Triangle* tri_indices2; - - details::RelativeTransformation RT; -}; - -/// @brief Traversal node for collision between two meshes if their underlying -/// BVH node is oriented node (OBB, RSS, OBBRSS, kIOS) -typedef MeshCollisionTraversalNode MeshCollisionTraversalNodeOBB; -typedef MeshCollisionTraversalNode MeshCollisionTraversalNodeRSS; -typedef MeshCollisionTraversalNode MeshCollisionTraversalNodekIOS; -typedef MeshCollisionTraversalNode MeshCollisionTraversalNodeOBBRSS; - -/// @} - -namespace details { -template -struct DistanceTraversalBVDistanceLowerBound_impl { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) { - return b1.distance(b2); - } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, - const BVNode& b2) { - return distance(R, T, b1.bv, b2.bv); - } -}; - -template <> -struct DistanceTraversalBVDistanceLowerBound_impl { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) { - FCL_REAL sqrDistLowerBound; - CollisionRequest request(DISTANCE_LOWER_BOUND, 0); - // request.break_distance = ? - if (b1.overlap(b2, request, sqrDistLowerBound)) { - // TODO A penetration upper bound should be computed. - return -1; - } - return sqrt(sqrDistLowerBound); - } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, - const BVNode& b2) { - FCL_REAL sqrDistLowerBound; - CollisionRequest request(DISTANCE_LOWER_BOUND, 0); - // request.break_distance = ? - if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { - // TODO A penetration upper bound should be computed. - return -1; - } - return sqrt(sqrDistLowerBound); - } -}; - -template <> -struct DistanceTraversalBVDistanceLowerBound_impl { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) { - FCL_REAL sqrDistLowerBound; - CollisionRequest request(DISTANCE_LOWER_BOUND, 0); - // request.break_distance = ? - if (b1.overlap(b2, request, sqrDistLowerBound)) { - // TODO A penetration upper bound should be computed. - return -1; - } - return sqrt(sqrDistLowerBound); - } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, - const BVNode& b2) { - FCL_REAL sqrDistLowerBound; - CollisionRequest request(DISTANCE_LOWER_BOUND, 0); - // request.break_distance = ? - if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { - // TODO A penetration upper bound should be computed. - return -1; - } - return sqrt(sqrDistLowerBound); - } -}; -} // namespace details - -/// @addtogroup Traversal_For_Distance -/// @{ - -/// @brief Traversal node for distance computation between BVH models -template -class BVHDistanceTraversalNode : public DistanceTraversalNodeBase { - public: - BVHDistanceTraversalNode() : DistanceTraversalNodeBase() { - model1 = NULL; - model2 = NULL; - - num_bv_tests = 0; - num_leaf_tests = 0; - query_time_seconds = 0.0; - } - - /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(unsigned int b) const { - return model1->getBV(b).isLeaf(); - } - - /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(unsigned int b) const { - return model2->getBV(b).isLeaf(); - } - - /// @brief Determine the traversal order, is the first BVTT subtree better - bool firstOverSecond(unsigned int b1, unsigned int b2) const { - FCL_REAL sz1 = model1->getBV(b1).bv.size(); - FCL_REAL sz2 = model2->getBV(b2).bv.size(); - - bool l1 = model1->getBV(b1).isLeaf(); - bool l2 = model2->getBV(b2).isLeaf(); - - if (l2 || (!l1 && (sz1 > sz2))) return true; - return false; - } - - /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(unsigned int b) const { - return model1->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(unsigned int b) const { - return model1->getBV(b).rightChild(); - } - - /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(unsigned int b) const { - return model2->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(unsigned int b) const { - return model2->getBV(b).rightChild(); - } - - /// @brief The first BVH model - const BVHModel* model1; - /// @brief The second BVH model - const BVHModel* model2; - - /// @brief statistical information - mutable int num_bv_tests; - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; -}; - -/// @brief Traversal node for distance computation between two meshes -template -class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { - public: - enum { - Options = _Options, - RTIsIdentity = _Options & RelativeTransformationIsIdentity - }; - - using BVHDistanceTraversalNode::enable_statistics; - using BVHDistanceTraversalNode::request; - using BVHDistanceTraversalNode::result; - using BVHDistanceTraversalNode::tf1; - using BVHDistanceTraversalNode::model1; - using BVHDistanceTraversalNode::model2; - using BVHDistanceTraversalNode::num_bv_tests; - using BVHDistanceTraversalNode::num_leaf_tests; - - MeshDistanceTraversalNode() : BVHDistanceTraversalNode() { - vertices1 = NULL; - vertices2 = NULL; - tri_indices1 = NULL; - tri_indices2 = NULL; - - rel_err = this->request.rel_err; - abs_err = this->request.abs_err; - } - - void preprocess() { - if (!RTIsIdentity) preprocessOrientedNode(); - } - - void postprocess() { - if (!RTIsIdentity) postprocessOrientedNode(); - } - - /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int b2) const { - if (enable_statistics) num_bv_tests++; - if (RTIsIdentity) - return details::DistanceTraversalBVDistanceLowerBound_impl::run( - model1->getBV(b1), model2->getBV(b2)); - else - return details::DistanceTraversalBVDistanceLowerBound_impl::run( - RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2)); - } - - /// @brief Distance testing between leaves (two triangles) - void leafComputeDistance(unsigned int b1, unsigned int b2) const { - if (this->enable_statistics) this->num_leaf_tests++; - - const BVNode& node1 = this->model1->getBV(b1); - const BVNode& node2 = this->model2->getBV(b2); - - int primitive_id1 = node1.primitiveId(); - int primitive_id2 = node2.primitiveId(); - - const Triangle& tri_id1 = tri_indices1[primitive_id1]; - const Triangle& tri_id2 = tri_indices2[primitive_id2]; - - const Vec3f& t11 = vertices1[tri_id1[0]]; - const Vec3f& t12 = vertices1[tri_id1[1]]; - const Vec3f& t13 = vertices1[tri_id1[2]]; - - const Vec3f& t21 = vertices2[tri_id2[0]]; - const Vec3f& t22 = vertices2[tri_id2[1]]; - const Vec3f& t23 = vertices2[tri_id2[2]]; - - // nearest point pair - Vec3f P1, P2, normal; - - FCL_REAL d2; - if (RTIsIdentity) - d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1, - P2); - else - d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, - RT._R(), RT._T(), P1, P2); - FCL_REAL d = sqrt(d2); - - this->result->update(d, this->model1, this->model2, primitive_id1, - primitive_id2, P1, P2, normal); - } - - /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const { - if ((c >= this->result->min_distance - abs_err) && - (c * (1 + rel_err) >= this->result->min_distance)) - return true; - return false; - } - - Vec3f* vertices1; - Vec3f* vertices2; - - Triangle* tri_indices1; - Triangle* tri_indices2; - - /// @brief relative and absolute error, default value is 0.01 for both terms - FCL_REAL rel_err; - FCL_REAL abs_err; - - details::RelativeTransformation RT; - - private: - void preprocessOrientedNode() { - const int init_tri_id1 = 0, init_tri_id2 = 0; - const Triangle& init_tri1 = tri_indices1[init_tri_id1]; - const Triangle& init_tri2 = tri_indices2[init_tri_id2]; - - Vec3f init_tri1_points[3]; - Vec3f init_tri2_points[3]; - - init_tri1_points[0] = vertices1[init_tri1[0]]; - init_tri1_points[1] = vertices1[init_tri1[1]]; - init_tri1_points[2] = vertices1[init_tri1[2]]; - - init_tri2_points[0] = vertices2[init_tri2[0]]; - init_tri2_points[1] = vertices2[init_tri2[1]]; - init_tri2_points[2] = vertices2[init_tri2[2]]; - - Vec3f p1, p2, normal; - FCL_REAL distance = sqrt(TriangleDistance::sqrTriDistance( - init_tri1_points[0], init_tri1_points[1], init_tri1_points[2], - init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(), - RT._T(), p1, p2)); - - result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2, - normal); - } - void postprocessOrientedNode() { - /// the points obtained by triDistance are not in world space: both are in - /// object1's local coordinate system, so we need to convert them into the - /// world space. - if (request.enable_nearest_points && (result->o1 == model1) && - (result->o2 == model2)) { - result->nearest_points[0] = tf1.transform(result->nearest_points[0]); - result->nearest_points[1] = tf1.transform(result->nearest_points[1]); - } - } -}; - -/// @brief Traversal node for distance computation between two meshes if their -/// underlying BVH node is oriented node (RSS, OBBRSS, kIOS) -typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeRSS; -typedef MeshDistanceTraversalNode MeshDistanceTraversalNodekIOS; -typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeOBBRSS; - -/// @} - -/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to -/// be transformed -namespace details { - -template -inline const Matrix3f& getBVAxes(const BV& bv) { - return bv.axes; -} - -template <> -inline const Matrix3f& getBVAxes(const OBBRSS& bv) { - return bv.obb.axes; -} - -} // namespace details - -} // namespace fcl - -} // namespace hpp - -/// @endcond - -#endif +#include +#include diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h index 8bd5617c6..8384a2b94 100644 --- a/include/hpp/fcl/internal/traversal_node_hfield_shape.h +++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h @@ -1,758 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2021-2024, INRIA. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_TRAVERSAL_NODE_HFIELD_SHAPE_H -#define HPP_FCL_TRAVERSAL_NODE_HFIELD_SHAPE_H - -/// @cond INTERNAL - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// @addtogroup Traversal_For_Collision -/// @{ - -namespace details { -template -Convex buildConvexQuadrilateral(const HFNode& node, - const HeightField& model) { - const MatrixXf& heights = model.getHeights(); - const VecXf& x_grid = model.getXGrid(); - const VecXf& y_grid = model.getYGrid(); - - const FCL_REAL min_height = model.getMinHeight(); - - const FCL_REAL x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], - y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; - const Eigen::Block cell = - heights.block<2, 2>(node.y_id, node.x_id); - - assert(cell.maxCoeff() > min_height && - "max_height is lower than min_height"); // Check whether the geometry - // is degenerated - - std::shared_ptr> pts(new std::vector({ - Vec3f(x0, y0, min_height), - Vec3f(x0, y1, min_height), - Vec3f(x1, y1, min_height), - Vec3f(x1, y0, min_height), - Vec3f(x0, y0, cell(0, 0)), - Vec3f(x0, y1, cell(1, 0)), - Vec3f(x1, y1, cell(1, 1)), - Vec3f(x1, y0, cell(0, 1)), - })); - - std::shared_ptr> polygons( - new std::vector(6)); - (*polygons)[0].set(0, 3, 2, 1); // x+ side - (*polygons)[1].set(0, 1, 5, 4); // y- side - (*polygons)[2].set(1, 2, 6, 5); // x- side - (*polygons)[3].set(2, 3, 7, 6); // y+ side - (*polygons)[4].set(3, 0, 4, 7); // z- side - (*polygons)[5].set(4, 5, 6, 7); // z+ side - - return Convex(pts, // points - 8, // num points - polygons, - 6 // number of polygons - ); -} - -enum class FaceOrientationConvexPart1 { - BOTTOM = 0, - TOP = 1, - WEST = 2, - SOUTH_EAST = 4, - NORTH = 8, -}; - -enum class FaceOrientationConvexPart2 { - BOTTOM = 0, - TOP = 1, - SOUTH = 2, - NORTH_WEST = 4, - EAST = 8, -}; - -template -void buildConvexTriangles(const HFNode& node, const HeightField& model, - Convex& convex1, int& convex1_active_faces, - Convex& convex2, - int& convex2_active_faces) { - const MatrixXf& heights = model.getHeights(); - const VecXf& x_grid = model.getXGrid(); - const VecXf& y_grid = model.getYGrid(); - - const FCL_REAL min_height = model.getMinHeight(); - - const FCL_REAL x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], - y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; - const FCL_REAL max_height = node.max_height; - const Eigen::Block cell = - heights.block<2, 2>(node.y_id, node.x_id); - - const int contact_active_faces = node.contact_active_faces; - convex1_active_faces = 0; - convex2_active_faces = 0; - - typedef HFNodeBase::FaceOrientation FaceOrientation; - - if (contact_active_faces & FaceOrientation::TOP) { - convex1_active_faces |= int(details::FaceOrientationConvexPart1::TOP); - convex2_active_faces |= int(details::FaceOrientationConvexPart2::TOP); - } - - if (contact_active_faces & FaceOrientation::BOTTOM) { - convex1_active_faces |= int(details::FaceOrientationConvexPart1::BOTTOM); - convex2_active_faces |= int(details::FaceOrientationConvexPart2::BOTTOM); - } - - // Specific orientation for Convex1 - if (contact_active_faces & FaceOrientation::WEST) { - convex1_active_faces |= int(details::FaceOrientationConvexPart1::WEST); - } - - if (contact_active_faces & FaceOrientation::NORTH) { - convex1_active_faces |= int(details::FaceOrientationConvexPart1::NORTH); - } - - // Specific orientation for Convex2 - if (contact_active_faces & FaceOrientation::EAST) { - convex2_active_faces |= int(details::FaceOrientationConvexPart2::EAST); - } - - if (contact_active_faces & FaceOrientation::SOUTH) { - convex2_active_faces |= int(details::FaceOrientationConvexPart2::SOUTH); - } - - assert(max_height > min_height && - "max_height is lower than min_height"); // Check whether the geometry - // is degenerated - HPP_FCL_UNUSED_VARIABLE(max_height); - - { - std::shared_ptr> pts(new std::vector({ - Vec3f(x0, y0, min_height), // A - Vec3f(x0, y1, min_height), // B - Vec3f(x1, y0, min_height), // C - Vec3f(x0, y0, cell(0, 0)), // D - Vec3f(x0, y1, cell(1, 0)), // E - Vec3f(x1, y0, cell(0, 1)), // F - })); - - std::shared_ptr> triangles( - new std::vector(8)); - (*triangles)[0].set(0, 2, 1); // bottom - (*triangles)[1].set(3, 4, 5); // top - (*triangles)[2].set(0, 1, 3); // West 1 - (*triangles)[3].set(3, 1, 4); // West 2 - (*triangles)[4].set(1, 2, 5); // South-East 1 - (*triangles)[5].set(1, 5, 4); // South-East 1 - (*triangles)[6].set(0, 5, 2); // North 1 - (*triangles)[7].set(5, 0, 3); // North 2 - - convex1.set(pts, // points - 6, // num points - triangles, - 8 // number of polygons - ); - } - - { - std::shared_ptr> pts(new std::vector({ - Vec3f(x0, y1, min_height), // A - Vec3f(x1, y1, min_height), // B - Vec3f(x1, y0, min_height), // C - Vec3f(x0, y1, cell(1, 0)), // D - Vec3f(x1, y1, cell(1, 1)), // E - Vec3f(x1, y0, cell(0, 1)), // F - })); - - std::shared_ptr> triangles( - new std::vector(8)); - (*triangles)[0].set(2, 1, 0); // bottom - (*triangles)[1].set(3, 4, 5); // top - (*triangles)[2].set(0, 1, 3); // South 1 - (*triangles)[3].set(3, 1, 4); // South 2 - (*triangles)[4].set(0, 5, 2); // North West 1 - (*triangles)[5].set(0, 3, 5); // North West 2 - (*triangles)[6].set(1, 2, 5); // East 1 - (*triangles)[7].set(4, 1, 2); // East 2 - - convex2.set(pts, // points - 6, // num points - triangles, - 8 // number of polygons - ); - } -} - -inline Vec3f projectTriangle(const Vec3f& pointA, const Vec3f& pointB, - const Vec3f& pointC, const Vec3f& point) { - const Project::ProjectResult result = - Project::projectTriangle(pointA, pointB, pointC, point); - Vec3f res = result.parameterization[0] * pointA + - result.parameterization[1] * pointB + - result.parameterization[2] * pointC; - - return res; -} - -inline Vec3f projectTetrahedra(const Vec3f& pointA, const Vec3f& pointB, - const Vec3f& pointC, const Vec3f& pointD, - const Vec3f& point) { - const Project::ProjectResult result = - Project::projectTetrahedra(pointA, pointB, pointC, pointD, point); - Vec3f res = result.parameterization[0] * pointA + - result.parameterization[1] * pointB + - result.parameterization[2] * pointC + - result.parameterization[3] * pointD; - - return res; -} - -inline Vec3f computeTriangleNormal(const Triangle& triangle, - const std::vector& points) { - const Vec3f pointA = points[triangle[0]]; - const Vec3f pointB = points[triangle[1]]; - const Vec3f pointC = points[triangle[2]]; - - const Vec3f normal = (pointB - pointA).cross(pointC - pointA).normalized(); - assert(!normal.array().isNaN().any() && "normal is ill-defined"); - - return normal; -} - -inline Vec3f projectPointOnTriangle(const Vec3f& contact_point, - const Triangle& triangle, - const std::vector& points) { - const Vec3f pointA = points[triangle[0]]; - const Vec3f pointB = points[triangle[1]]; - const Vec3f pointC = points[triangle[2]]; - - const Vec3f contact_point_projected = - projectTriangle(pointA, pointB, pointC, contact_point); - - return contact_point_projected; -} - -inline FCL_REAL distanceContactPointToTriangle( - const Vec3f& contact_point, const Triangle& triangle, - const std::vector& points) { - const Vec3f contact_point_projected = - projectPointOnTriangle(contact_point, triangle, points); - return (contact_point_projected - contact_point).norm(); -} - -inline FCL_REAL distanceContactPointToFace(const size_t face_id, - const Vec3f& contact_point, - const Convex& convex, - size_t& closest_face_id) { - assert((face_id >= 0 && face_id < 8) && "face_id should be in [0;7]"); - - const std::vector& points = *(convex.points); - if (face_id <= 1) { - const Triangle& triangle = (*(convex.polygons))[face_id]; - closest_face_id = face_id; - return distanceContactPointToTriangle(contact_point, triangle, points); - } else { - const Triangle& triangle1 = (*(convex.polygons))[face_id]; - const FCL_REAL distance_to_triangle1 = - distanceContactPointToTriangle(contact_point, triangle1, points); - - const Triangle& triangle2 = (*(convex.polygons))[face_id + 1]; - const FCL_REAL distance_to_triangle2 = - distanceContactPointToTriangle(contact_point, triangle2, points); - - if (distance_to_triangle1 > distance_to_triangle2) { - closest_face_id = face_id + 1; - return distance_to_triangle2; - } else { - closest_face_id = face_id; - return distance_to_triangle1; - } - } -} - -template -bool binCorrection(const Convex& convex, - const int convex_active_faces, const Shape& shape, - const Transform3f& shape_pose, FCL_REAL& distance, - Vec3f& contact_1, Vec3f& contact_2, Vec3f& normal, - Vec3f& face_normal, const bool is_collision) { - const FCL_REAL prec = 1e-12; - const std::vector& points = *(convex.points); - - bool hfield_witness_is_on_bin_side = true; - - // int closest_face_id_bottom_face = -1; - // int closest_face_id_top_face = -1; - - std::vector active_faces; - active_faces.reserve(5); - active_faces.push_back(0); - active_faces.push_back(1); - - if (convex_active_faces & 2) active_faces.push_back(2); - if (convex_active_faces & 4) active_faces.push_back(4); - if (convex_active_faces & 8) active_faces.push_back(6); - - Triangle face_triangle; - FCL_REAL shortest_distance_to_face = (std::numeric_limits::max)(); - face_normal = normal; - for (const size_t active_face : active_faces) { - size_t closest_face_id; - const FCL_REAL distance_to_face = distanceContactPointToFace( - active_face, contact_1, convex, closest_face_id); - - const bool contact_point_is_on_face = distance_to_face <= prec; - if (contact_point_is_on_face) { - hfield_witness_is_on_bin_side = false; - face_triangle = (*(convex.polygons))[closest_face_id]; - shortest_distance_to_face = distance_to_face; - break; - } else if (distance_to_face < shortest_distance_to_face) { - face_triangle = (*(convex.polygons))[closest_face_id]; - shortest_distance_to_face = distance_to_face; - } - } - - // We correct only if there is a collision with the bin - if (is_collision) { - if (!face_triangle.isValid()) - HPP_FCL_THROW_PRETTY("face_triangle is not initialized", - std::logic_error); - - const Vec3f face_pointA = points[face_triangle[0]]; - face_normal = computeTriangleNormal(face_triangle, points); - - int hint = 0; - // Since we compute the support manually, we need to take into account the - // sphere swept radius of the shape. - // TODO: take into account the swept-sphere radius of the bin. - const Vec3f _support = getSupport( - &shape, -shape_pose.rotation().transpose() * face_normal, hint); - const Vec3f support = - shape_pose.rotation() * _support + shape_pose.translation(); - - // Project support into the inclined bin having triangle - const FCL_REAL offset_plane = face_normal.dot(face_pointA); - const Plane projection_plane(face_normal, offset_plane); - const FCL_REAL distance_support_projection_plane = - projection_plane.signedDistance(support); - - const Vec3f projected_support = - support - distance_support_projection_plane * face_normal; - - // We need now to project the projected in the triangle shape - contact_1 = - projectPointOnTriangle(projected_support, face_triangle, points); - contact_2 = contact_1 + distance_support_projection_plane * face_normal; - normal = face_normal; - distance = -std::fabs(distance_support_projection_plane); - } - - return hfield_witness_is_on_bin_side; -} - -template -bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request, - const Convex& convex1, - const int convex1_active_faces, - const Convex& convex2, - const int convex2_active_faces, const Transform3f& tf1, - const Shape& shape, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& c1, Vec3f& c2, Vec3f& normal, - Vec3f& normal_top, bool& hfield_witness_is_on_bin_side) { - enum { RTIsIdentity = Options & RelativeTransformationIsIdentity }; - - const Transform3f Id; - // The solver `nsolver` has already been set up by the collision request - // `request`. If GJK early stopping is enabled through `request`, it will be - // used. - // The only thing we need to make sure is that in case of collision, the - // penetration information is computed (as we do bins comparison). - const bool compute_penetration = true; - Vec3f contact1_1, contact1_2, contact2_1, contact2_2; - Vec3f normal1, normal1_top, normal2, normal2_top; - FCL_REAL distance1, distance2; - - if (RTIsIdentity) { - distance1 = internal::ShapeShapeDistance, Shape>( - &convex1, Id, &shape, tf2, nsolver, compute_penetration, contact1_1, - contact1_2, normal1); - } else { - distance1 = internal::ShapeShapeDistance, Shape>( - &convex1, tf1, &shape, tf2, nsolver, compute_penetration, contact1_1, - contact1_2, normal1); - } - bool collision1 = (distance1 - request.security_margin <= - request.collision_distance_threshold); - - bool hfield_witness_is_on_bin_side1 = - binCorrection(convex1, convex1_active_faces, shape, tf2, distance1, - contact1_1, contact1_2, normal1, normal1_top, collision1); - - if (RTIsIdentity) { - distance2 = internal::ShapeShapeDistance, Shape>( - &convex2, Id, &shape, tf2, nsolver, compute_penetration, contact2_1, - contact2_2, normal2); - } else { - distance2 = internal::ShapeShapeDistance, Shape>( - &convex2, tf1, &shape, tf2, nsolver, compute_penetration, contact2_1, - contact2_2, normal2); - } - bool collision2 = (distance2 - request.security_margin <= - request.collision_distance_threshold); - - bool hfield_witness_is_on_bin_side2 = - binCorrection(convex2, convex2_active_faces, shape, tf2, distance2, - contact2_1, contact2_2, normal2, normal2_top, collision2); - - if (collision1 && collision2) { - if (distance1 > distance2) // switch values - { - distance = distance2; - c1 = contact2_1; - c2 = contact2_2; - normal = normal2; - normal_top = normal2_top; - hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2; - } else { - distance = distance1; - c1 = contact1_1; - c2 = contact1_2; - normal = normal1; - normal_top = normal1_top; - hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1; - } - return true; - } else if (collision1) { - distance = distance1; - c1 = contact1_1; - c2 = contact1_2; - normal = normal1; - normal_top = normal1_top; - hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1; - return true; - } else if (collision2) { - distance = distance2; - c1 = contact2_1; - c2 = contact2_2; - normal = normal2; - normal_top = normal2_top; - hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2; - return true; - } - - if (distance1 > distance2) // switch values - { - distance = distance2; - c1 = contact2_1; - c2 = contact2_2; - normal = normal2; - normal_top = normal2_top; - hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2; - } else { - distance = distance1; - c1 = contact1_1; - c2 = contact1_2; - normal = normal1; - normal_top = normal1_top; - hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1; - } - return false; -} - -} // namespace details - -/// @brief Traversal node for collision between height field and shape -template -class HeightFieldShapeCollisionTraversalNode - : public CollisionTraversalNodeBase { - public: - typedef CollisionTraversalNodeBase Base; - typedef Eigen::Array Array2d; - - enum { - Options = _Options, - RTIsIdentity = _Options & RelativeTransformationIsIdentity - }; - - HeightFieldShapeCollisionTraversalNode(const CollisionRequest& request) - : CollisionTraversalNodeBase(request) { - model1 = NULL; - model2 = NULL; - - num_bv_tests = 0; - num_leaf_tests = 0; - query_time_seconds = 0.0; - - nsolver = NULL; - count = 0; - } - - /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(unsigned int b) const { - return model1->getBV(b).isLeaf(); - } - - /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(unsigned int b) const { - return static_cast(model1->getBV(b).leftChild()); - } - - /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(unsigned int b) const { - return static_cast(model1->getBV(b).rightChild()); - } - - /// test between BV b1 and shape - /// @param b1 BV to test, - /// @retval sqrDistLowerBound square of a lower bound of the minimal - /// distance between bounding volumes. - /// @brief BV culling test in one BVTT node - bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, - FCL_REAL& sqrDistLowerBound) const { - if (this->enable_statistics) this->num_bv_tests++; - - bool disjoint; - if (RTIsIdentity) { - assert(false && "must never happened"); - disjoint = !this->model1->getBV(b1).bv.overlap( - this->model2_bv, this->request, sqrDistLowerBound); - } else { - disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), - this->model1->getBV(b1).bv, this->model2_bv, - this->request, sqrDistLowerBound); - } - - if (disjoint) - internal::updateDistanceLowerBoundFromBV(this->request, *this->result, - sqrDistLowerBound); - - assert(!disjoint || sqrDistLowerBound > 0); - return disjoint; - } - - /// @brief Intersection testing between leaves (one Convex and one shape) - void leafCollides(unsigned int b1, unsigned int /*b2*/, - FCL_REAL& sqrDistLowerBound) const { - count++; - if (this->enable_statistics) this->num_leaf_tests++; - const HFNode& node = this->model1->getBV(b1); - - // Split quadrilateral primitives into two convex shapes corresponding to - // polyhedron with triangular bases. This is essential to keep the convexity - - // typedef Convex ConvexQuadrilateral; - // const ConvexQuadrilateral convex = - // details::buildConvexQuadrilateral(node,*this->model1); - - typedef Convex ConvexTriangle; - ConvexTriangle convex1, convex2; - int convex1_active_faces, convex2_active_faces; - // TODO: inherit from hfield's inflation here - details::buildConvexTriangles(node, *this->model1, convex1, - convex1_active_faces, convex2, - convex2_active_faces); - - // Compute aabb_local for BoundingVolumeGuess case in the GJK solver - if (nsolver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { - convex1.computeLocalAABB(); - convex2.computeLocalAABB(); - } - - FCL_REAL distance; - // Vec3f contact_point, normal; - Vec3f c1, c2, normal, normal_face; - bool hfield_witness_is_on_bin_side; - - bool collision = details::shapeDistance( - nsolver, this->request, convex1, convex1_active_faces, convex2, - convex2_active_faces, this->tf1, *(this->model2), this->tf2, distance, - c1, c2, normal, normal_face, hfield_witness_is_on_bin_side); - - FCL_REAL distToCollision = distance - this->request.security_margin; - if (distToCollision <= this->request.collision_distance_threshold) { - sqrDistLowerBound = 0; - if (this->result->numContacts() < this->request.num_max_contacts) { - if (normal_face.isApprox(normal) && - (collision || !hfield_witness_is_on_bin_side)) { - this->result->addContact(Contact(this->model1, this->model2, (int)b1, - (int)Contact::NONE, c1, c2, normal, - distance)); - assert(this->result->isCollision()); - } - } - } else - sqrDistLowerBound = distToCollision * distToCollision; - - // const Vec3f c1 = contact_point - distance * 0.5 * normal; - // const Vec3f c2 = contact_point + distance * 0.5 * normal; - internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result, - distToCollision, c1, c2, normal); - - assert(this->result->isCollision() || sqrDistLowerBound > 0); - } - - const GJKSolver* nsolver; - - const HeightField* model1; - const S* model2; - BV model2_bv; - - mutable int num_bv_tests; - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; - mutable int count; -}; - -/// @} - -/// @addtogroup Traversal_For_Distance -/// @{ - -/// @brief Traversal node for distance between height field and shape -template -class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase { - public: - typedef DistanceTraversalNodeBase Base; - - enum { - Options = _Options, - RTIsIdentity = _Options & RelativeTransformationIsIdentity - }; - - HeightFieldShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { - model1 = NULL; - model2 = NULL; - - num_leaf_tests = 0; - query_time_seconds = 0.0; - - rel_err = 0; - abs_err = 0; - nsolver = NULL; - } - - /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(unsigned int b) const { - return model1->getBV(b).isLeaf(); - } - - /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(unsigned int b) const { - return model1->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(unsigned int b) const { - return model1->getBV(b).rightChild(); - } - - /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { - return model1->getBV(b1).bv.distance( - model2_bv); // TODO(jcarpent): tf1 is not taken into account here. - } - - /// @brief Distance testing between leaves (one bin of the height field and - /// one shape) - /// TODO(louis): deal with Hfield-Shape distance just like in Hfield-Shape - /// collision (bin correction etc). - void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const { - if (this->enable_statistics) this->num_leaf_tests++; - - const BVNode& node = this->model1->getBV(b1); - - typedef Convex ConvexQuadrilateral; - const ConvexQuadrilateral convex = - details::buildConvexQuadrilateral(node, *this->model1); - - Vec3f p1, p2, normal; - const FCL_REAL distance = - internal::ShapeShapeDistance( - &convex, this->tf1, this->model2, this->tf2, this->nsolver, - this->request.enable_signed_distance, p1, p2, normal); - - this->result->update(distance, this->model1, this->model2, b1, - DistanceResult::NONE, p1, p2, normal); - } - - /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const { - if ((c >= this->result->min_distance - abs_err) && - (c * (1 + rel_err) >= this->result->min_distance)) - return true; - return false; - } - - FCL_REAL rel_err; - FCL_REAL abs_err; - - const GJKSolver* nsolver; - - const HeightField* model1; - const S* model2; - BV model2_bv; - - mutable int num_bv_tests; - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; -}; - -/// @} - -} // namespace fcl -} // namespace hpp - -/// @endcond - -#endif +#include +#include diff --git a/include/hpp/fcl/internal/traversal_node_octree.h b/include/hpp/fcl/internal/traversal_node_octree.h index 9a0094e13..44f8b3234 100644 --- a/include/hpp/fcl/internal/traversal_node_octree.h +++ b/include/hpp/fcl/internal/traversal_node_octree.h @@ -1,1363 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * Copyright (c) 2022-2023, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_TRAVERSAL_NODE_OCTREE_H -#define HPP_FCL_TRAVERSAL_NODE_OCTREE_H - -/// @cond INTERNAL - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// @brief Algorithms for collision related with octree -class HPP_FCL_DLLAPI OcTreeSolver { - private: - const GJKSolver* solver; - - mutable const CollisionRequest* crequest; - mutable const DistanceRequest* drequest; - - mutable CollisionResult* cresult; - mutable DistanceResult* dresult; - - public: - OcTreeSolver(const GJKSolver* solver_) - : solver(solver_), - crequest(NULL), - drequest(NULL), - cresult(NULL), - dresult(NULL) {} - - /// @brief collision between two octrees - void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2, - const Transform3f& tf1, const Transform3f& tf2, - const CollisionRequest& request_, - CollisionResult& result_) const { - crequest = &request_; - cresult = &result_; - - OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, - tree2->getRoot(), tree2->getRootBV(), tf1, tf2); - } - - /// @brief distance between two octrees - void OcTreeDistance(const OcTree* tree1, const OcTree* tree2, - const Transform3f& tf1, const Transform3f& tf2, - const DistanceRequest& request_, - DistanceResult& result_) const { - drequest = &request_; - dresult = &result_; - - OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, - tree2->getRoot(), tree2->getRootBV(), tf1, tf2); - } - - /// @brief collision between octree and mesh - template - void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel* tree2, - const Transform3f& tf1, const Transform3f& tf2, - const CollisionRequest& request_, - CollisionResult& result_) const { - crequest = &request_; - cresult = &result_; - - OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), - tree2, 0, tf1, tf2); - } - - /// @brief distance between octree and mesh - template - void OcTreeMeshDistance(const OcTree* tree1, const BVHModel* tree2, - const Transform3f& tf1, const Transform3f& tf2, - const DistanceRequest& request_, - DistanceResult& result_) const { - drequest = &request_; - dresult = &result_; - - OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), - tree2, 0, tf1, tf2); - } - - /// @brief collision between mesh and octree - template - void MeshOcTreeIntersect(const BVHModel* tree1, const OcTree* tree2, - const Transform3f& tf1, const Transform3f& tf2, - const CollisionRequest& request_, - CollisionResult& result_) const - - { - crequest = &request_; - cresult = &result_; - - OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(), - tree1, 0, tf2, tf1); - } - - /// @brief distance between mesh and octree - template - void MeshOcTreeDistance(const BVHModel* tree1, const OcTree* tree2, - const Transform3f& tf1, const Transform3f& tf2, - const DistanceRequest& request_, - DistanceResult& result_) const { - drequest = &request_; - dresult = &result_; - - OcTreeMeshDistanceRecurse(tree1, 0, tree2, tree2->getRoot(), - tree2->getRootBV(), tf1, tf2); - } - - template - void OcTreeHeightFieldIntersect( - const OcTree* tree1, const HeightField* tree2, const Transform3f& tf1, - const Transform3f& tf2, const CollisionRequest& request_, - CollisionResult& result_, FCL_REAL& sqrDistLowerBound) const { - crequest = &request_; - cresult = &result_; - - OcTreeHeightFieldIntersectRecurse(tree1, tree1->getRoot(), - tree1->getRootBV(), tree2, 0, tf1, tf2, - sqrDistLowerBound); - } - - template - void HeightFieldOcTreeIntersect(const HeightField* tree1, - const OcTree* tree2, const Transform3f& tf1, - const Transform3f& tf2, - const CollisionRequest& request_, - CollisionResult& result_, - FCL_REAL& sqrDistLowerBound) const { - crequest = &request_; - cresult = &result_; - - HeightFieldOcTreeIntersectRecurse(tree1, 0, tree2, tree2->getRoot(), - tree2->getRootBV(), tf1, tf2, - sqrDistLowerBound); - } - - /// @brief collision between octree and shape - template - void OcTreeShapeIntersect(const OcTree* tree, const S& s, - const Transform3f& tf1, const Transform3f& tf2, - const CollisionRequest& request_, - CollisionResult& result_) const { - crequest = &request_; - cresult = &result_; - - AABB bv2; - computeBV(s, Transform3f(), bv2); - OBB obb2; - convertBV(bv2, tf2, obb2); - OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s, - obb2, tf1, tf2); - } - - /// @brief collision between shape and octree - template - void ShapeOcTreeIntersect(const S& s, const OcTree* tree, - const Transform3f& tf1, const Transform3f& tf2, - const CollisionRequest& request_, - CollisionResult& result_) const { - crequest = &request_; - cresult = &result_; - - AABB bv1; - computeBV(s, Transform3f(), bv1); - OBB obb1; - convertBV(bv1, tf1, obb1); - OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s, - obb1, tf2, tf1); - } - - /// @brief distance between octree and shape - template - void OcTreeShapeDistance(const OcTree* tree, const S& s, - const Transform3f& tf1, const Transform3f& tf2, - const DistanceRequest& request_, - DistanceResult& result_) const { - drequest = &request_; - dresult = &result_; - - AABB aabb2; - computeBV(s, tf2, aabb2); - OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s, - aabb2, tf1, tf2); - } - - /// @brief distance between shape and octree - template - void ShapeOcTreeDistance(const S& s, const OcTree* tree, - const Transform3f& tf1, const Transform3f& tf2, - const DistanceRequest& request_, - DistanceResult& result_) const { - drequest = &request_; - dresult = &result_; - - AABB aabb1; - computeBV(s, tf1, aabb1); - OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s, - aabb1, tf2, tf1); - } - - private: - template - bool OcTreeShapeDistanceRecurse(const OcTree* tree1, - const OcTree::OcTreeNode* root1, - const AABB& bv1, const S& s, - const AABB& aabb2, const Transform3f& tf1, - const Transform3f& tf2) const { - if (!tree1->nodeHasChildren(root1)) { - if (tree1->isNodeOccupied(root1)) { - Box box; - Transform3f box_tf; - constructBox(bv1, tf1, box, box_tf); - - if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { - box.computeLocalAABB(); - } - - Vec3f p1, p2, normal; - const FCL_REAL distance = internal::ShapeShapeDistance( - &box, box_tf, &s, tf2, this->solver, - this->drequest->enable_signed_distance, p1, p2, normal); - - this->dresult->update(distance, tree1, &s, - (int)(root1 - tree1->getRoot()), - DistanceResult::NONE, p1, p2, normal); - - return drequest->isSatisfied(*dresult); - } else - return false; - } - - if (!tree1->isNodeOccupied(root1)) return false; - - for (unsigned int i = 0; i < 8; ++i) { - if (tree1->nodeChildExists(root1, i)) { - const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); - AABB child_bv; - computeChildBV(bv1, i, child_bv); - - AABB aabb1; - convertBV(child_bv, tf1, aabb1); - FCL_REAL d = aabb1.distance(aabb2); - if (d < dresult->min_distance) { - if (OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, - tf2)) - return true; - } - } - } - - return false; - } - - template - bool OcTreeShapeIntersectRecurse(const OcTree* tree1, - const OcTree::OcTreeNode* root1, - const AABB& bv1, const S& s, const OBB& obb2, - const Transform3f& tf1, - const Transform3f& tf2) const { - // Empty OcTree is considered free. - if (!root1) return false; - - /// stop when 1) bounding boxes of two objects not overlap; OR - /// 2) at least of one the nodes is free; OR - /// 2) (two uncertain nodes or one node occupied and one node - /// uncertain) AND cost not required - if (tree1->isNodeFree(root1)) - return false; - else if ((tree1->isNodeUncertain(root1) || s.isUncertain())) - return false; - else { - OBB obb1; - convertBV(bv1, tf1, obb1); - FCL_REAL sqrDistLowerBound; - if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { - internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, - sqrDistLowerBound); - return false; - } - } - - if (!tree1->nodeHasChildren(root1)) { - assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. - - Box box; - Transform3f box_tf; - constructBox(bv1, tf1, box, box_tf); - if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { - box.computeLocalAABB(); - } - - bool contactNotAdded = - (cresult->numContacts() >= crequest->num_max_contacts); - std::size_t ncontact = ShapeShapeCollide( - &box, box_tf, &s, tf2, solver, *crequest, *cresult); - assert(ncontact == 0 || ncontact == 1); - if (!contactNotAdded && ncontact == 1) { - // Update contact information. - const Contact& c = cresult->getContact(cresult->numContacts() - 1); - cresult->setContact( - cresult->numContacts() - 1, - Contact(tree1, c.o2, static_cast(root1 - tree1->getRoot()), - c.b2, c.pos, c.normal, c.penetration_depth)); - } - - // no need to call `internal::updateDistanceLowerBoundFromLeaf` here - // as it is already done internally in `ShapeShapeCollide` above. - return crequest->isSatisfied(*cresult); - } - - for (unsigned int i = 0; i < 8; ++i) { - if (tree1->nodeChildExists(root1, i)) { - const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); - AABB child_bv; - computeChildBV(bv1, i, child_bv); - - if (OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, - tf2)) - return true; - } - } - - return false; - } - - template - bool OcTreeMeshDistanceRecurse(const OcTree* tree1, - const OcTree::OcTreeNode* root1, - const AABB& bv1, const BVHModel* tree2, - unsigned int root2, const Transform3f& tf1, - const Transform3f& tf2) const { - if (!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) { - if (tree1->isNodeOccupied(root1)) { - Box box; - Transform3f box_tf; - constructBox(bv1, tf1, box, box_tf); - - if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { - box.computeLocalAABB(); - } - - size_t primitive_id = - static_cast(tree2->getBV(root2).primitiveId()); - const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id]; - const TriangleP tri((*(tree2->vertices))[tri_id[0]], - (*(tree2->vertices))[tri_id[1]], - (*(tree2->vertices))[tri_id[2]]); - - Vec3f p1, p2, normal; - const FCL_REAL distance = internal::ShapeShapeDistance( - &box, box_tf, &tri, tf2, this->solver, - this->drequest->enable_signed_distance, p1, p2, normal); - - this->dresult->update(distance, tree1, tree2, - (int)(root1 - tree1->getRoot()), - static_cast(primitive_id), p1, p2, normal); - - return this->drequest->isSatisfied(*dresult); - } else - return false; - } - - if (!tree1->isNodeOccupied(root1)) return false; - - if (tree2->getBV(root2).isLeaf() || - (tree1->nodeHasChildren(root1) && - (bv1.size() > tree2->getBV(root2).bv.size()))) { - for (unsigned int i = 0; i < 8; ++i) { - if (tree1->nodeChildExists(root1, i)) { - const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); - AABB child_bv; - computeChildBV(bv1, i, child_bv); - - FCL_REAL d; - AABB aabb1, aabb2; - convertBV(child_bv, tf1, aabb1); - convertBV(tree2->getBV(root2).bv, tf2, aabb2); - d = aabb1.distance(aabb2); - - if (d < dresult->min_distance) { - if (OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, - tf1, tf2)) - return true; - } - } - } - } else { - FCL_REAL d; - AABB aabb1, aabb2; - convertBV(bv1, tf1, aabb1); - unsigned int child = (unsigned int)tree2->getBV(root2).leftChild(); - convertBV(tree2->getBV(child).bv, tf2, aabb2); - d = aabb1.distance(aabb2); - - if (d < dresult->min_distance) { - if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, - tf2)) - return true; - } - - child = (unsigned int)tree2->getBV(root2).rightChild(); - convertBV(tree2->getBV(child).bv, tf2, aabb2); - d = aabb1.distance(aabb2); - - if (d < dresult->min_distance) { - if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, - tf2)) - return true; - } - } - - return false; - } - - /// \return True if the request is satisfied. - template - bool OcTreeMeshIntersectRecurse(const OcTree* tree1, - const OcTree::OcTreeNode* root1, - const AABB& bv1, const BVHModel* tree2, - unsigned int root2, const Transform3f& tf1, - const Transform3f& tf2) const { - // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The - // code in this if(!root1) did not output anything so the empty OcTree is - // considered free. Should an empty OcTree be considered free ? - - // Empty OcTree is considered free. - if (!root1) return false; - BVNode const& bvn2 = tree2->getBV(root2); - - /// stop when 1) bounding boxes of two objects not overlap; OR - /// 2) at least one of the nodes is free; OR - /// 2) (two uncertain nodes OR one node occupied and one node - /// uncertain) AND cost not required - if (tree1->isNodeFree(root1)) - return false; - else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain())) - return false; - else { - OBB obb1, obb2; - convertBV(bv1, tf1, obb1); - convertBV(bvn2.bv, tf2, obb2); - FCL_REAL sqrDistLowerBound; - if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { - internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, - sqrDistLowerBound); - return false; - } - } - - // Check if leaf collides. - if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) { - assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. - Box box; - Transform3f box_tf; - constructBox(bv1, tf1, box, box_tf); - if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { - box.computeLocalAABB(); - } - - size_t primitive_id = static_cast(bvn2.primitiveId()); - const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id]; - const TriangleP tri((*(tree2->vertices))[tri_id[0]], - (*(tree2->vertices))[tri_id[1]], - (*(tree2->vertices))[tri_id[2]]); - - // When reaching this point, `this->solver` has already been set up - // by the CollisionRequest `this->crequest`. - // The only thing we need to (and can) pass to `ShapeShapeDistance` is - // whether or not penetration information is should be computed in case of - // collision. - const bool compute_penetration = this->crequest->enable_contact || - (this->crequest->security_margin < 0); - Vec3f c1, c2, normal; - const FCL_REAL distance = internal::ShapeShapeDistance( - &box, box_tf, &tri, tf2, this->solver, compute_penetration, c1, c2, - normal); - const FCL_REAL distToCollision = - distance - this->crequest->security_margin; - - internal::updateDistanceLowerBoundFromLeaf( - *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal); - - if (cresult->numContacts() < crequest->num_max_contacts) { - if (distToCollision <= crequest->collision_distance_threshold) { - cresult->addContact(Contact( - tree1, tree2, (int)(root1 - tree1->getRoot()), - static_cast(primitive_id), c1, c2, normal, distance)); - } - } - return crequest->isSatisfied(*cresult); - } - - // Determine which tree to traverse first. - if (bvn2.isLeaf() || - (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) { - for (unsigned int i = 0; i < 8; ++i) { - if (tree1->nodeChildExists(root1, i)) { - const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); - AABB child_bv; - computeChildBV(bv1, i, child_bv); - - if (OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, - tf1, tf2)) - return true; - } - } - } else { - if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, - (unsigned int)bvn2.leftChild(), tf1, tf2)) - return true; - - if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, - (unsigned int)bvn2.rightChild(), tf1, tf2)) - return true; - } - - return false; - } - - /// \return True if the request is satisfied. - template - bool OcTreeHeightFieldIntersectRecurse( - const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, - const HeightField* tree2, unsigned int root2, const Transform3f& tf1, - const Transform3f& tf2, FCL_REAL& sqrDistLowerBound) const { - // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The - // code in this if(!root1) did not output anything so the empty OcTree is - // considered free. Should an empty OcTree be considered free ? - - // Empty OcTree is considered free. - if (!root1) return false; - HFNode const& bvn2 = tree2->getBV(root2); - - /// stop when 1) bounding boxes of two objects not overlap; OR - /// 2) at least one of the nodes is free; OR - /// 2) (two uncertain nodes OR one node occupied and one node - /// uncertain) AND cost not required - if (tree1->isNodeFree(root1)) - return false; - else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain())) - return false; - else { - OBB obb1, obb2; - convertBV(bv1, tf1, obb1); - convertBV(bvn2.bv, tf2, obb2); - FCL_REAL sqrDistLowerBound_; - if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound_)) { - if (sqrDistLowerBound_ < sqrDistLowerBound) - sqrDistLowerBound = sqrDistLowerBound_; - internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, - sqrDistLowerBound); - return false; - } - } - - // Check if leaf collides. - if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) { - assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. - Box box; - Transform3f box_tf; - constructBox(bv1, tf1, box, box_tf); - if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { - box.computeLocalAABB(); - } - - typedef Convex ConvexTriangle; - ConvexTriangle convex1, convex2; - int convex1_active_faces, convex2_active_faces; - details::buildConvexTriangles(bvn2, *tree2, convex1, convex1_active_faces, - convex2, convex2_active_faces); - if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { - convex1.computeLocalAABB(); - convex2.computeLocalAABB(); - } - - Vec3f c1, c2, normal, normal_top; - FCL_REAL distance; - bool hfield_witness_is_on_bin_side; - - bool collision = details::shapeDistance( - solver, *crequest, convex1, convex1_active_faces, convex2, - convex2_active_faces, tf2, box, box_tf, distance, c2, c1, normal, - normal_top, hfield_witness_is_on_bin_side); - - FCL_REAL distToCollision = - distance - crequest->security_margin * (normal_top.dot(normal)); - - if (distToCollision <= crequest->collision_distance_threshold) { - sqrDistLowerBound = 0; - if (crequest->num_max_contacts > cresult->numContacts()) { - if (normal_top.isApprox(normal) && - (collision || !hfield_witness_is_on_bin_side)) { - cresult->addContact( - Contact(tree1, tree2, (int)(root1 - tree1->getRoot()), - (int)Contact::NONE, c1, c2, -normal, distance)); - } - } - } else - sqrDistLowerBound = distToCollision * distToCollision; - - // const Vec3f c1 = contact_point - distance * 0.5 * normal; - // const Vec3f c2 = contact_point + distance * 0.5 * normal; - internal::updateDistanceLowerBoundFromLeaf( - *crequest, *cresult, distToCollision, c1, c2, -normal); - - assert(cresult->isCollision() || sqrDistLowerBound > 0); - return crequest->isSatisfied(*cresult); - } - - // Determine which tree to traverse first. - if (bvn2.isLeaf() || - (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) { - for (unsigned int i = 0; i < 8; ++i) { - if (tree1->nodeChildExists(root1, i)) { - const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); - AABB child_bv; - computeChildBV(bv1, i, child_bv); - - if (OcTreeHeightFieldIntersectRecurse(tree1, child, child_bv, tree2, - root2, tf1, tf2, - sqrDistLowerBound)) - return true; - } - } - } else { - if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2, - (unsigned int)bvn2.leftChild(), tf1, - tf2, sqrDistLowerBound)) - return true; - - if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2, - (unsigned int)bvn2.rightChild(), - tf1, tf2, sqrDistLowerBound)) - return true; - } - - return false; - } - - /// \return True if the request is satisfied. - template - bool HeightFieldOcTreeIntersectRecurse( - const HeightField* tree1, unsigned int root1, const OcTree* tree2, - const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3f& tf1, - const Transform3f& tf2, FCL_REAL& sqrDistLowerBound) const { - // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The - // code in this if(!root1) did not output anything so the empty OcTree is - // considered free. Should an empty OcTree be considered free ? - - // Empty OcTree is considered free. - if (!root2) return false; - HFNode const& bvn1 = tree1->getBV(root1); - - /// stop when 1) bounding boxes of two objects not overlap; OR - /// 2) at least one of the nodes is free; OR - /// 2) (two uncertain nodes OR one node occupied and one node - /// uncertain) AND cost not required - if (tree2->isNodeFree(root2)) - return false; - else if ((tree2->isNodeUncertain(root2) || tree1->isUncertain())) - return false; - else { - OBB obb1, obb2; - convertBV(bvn1.bv, tf1, obb1); - convertBV(bv2, tf2, obb2); - FCL_REAL sqrDistLowerBound_; - if (!obb2.overlap(obb1, *crequest, sqrDistLowerBound_)) { - if (sqrDistLowerBound_ < sqrDistLowerBound) - sqrDistLowerBound = sqrDistLowerBound_; - internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, - sqrDistLowerBound); - return false; - } - } - - // Check if leaf collides. - if (!tree2->nodeHasChildren(root2) && bvn1.isLeaf()) { - assert(tree2->isNodeOccupied(root2)); // it isn't free nor uncertain. - Box box; - Transform3f box_tf; - constructBox(bv2, tf2, box, box_tf); - if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { - box.computeLocalAABB(); - } - - typedef Convex ConvexTriangle; - ConvexTriangle convex1, convex2; - int convex1_active_faces, convex2_active_faces; - details::buildConvexTriangles(bvn1, *tree1, convex1, convex1_active_faces, - convex2, convex2_active_faces); - if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { - convex1.computeLocalAABB(); - convex2.computeLocalAABB(); - } - - Vec3f c1, c2, normal, normal_top; - FCL_REAL distance; - bool hfield_witness_is_on_bin_side; - - bool collision = details::shapeDistance( - solver, *crequest, convex1, convex1_active_faces, convex2, - convex2_active_faces, tf1, box, box_tf, distance, c1, c2, normal, - normal_top, hfield_witness_is_on_bin_side); - - FCL_REAL distToCollision = - distance - crequest->security_margin * (normal_top.dot(normal)); - - if (distToCollision <= crequest->collision_distance_threshold) { - sqrDistLowerBound = 0; - if (crequest->num_max_contacts > cresult->numContacts()) { - if (normal_top.isApprox(normal) && - (collision || !hfield_witness_is_on_bin_side)) { - cresult->addContact(Contact(tree1, tree2, (int)Contact::NONE, - (int)(root2 - tree2->getRoot()), c1, c2, - normal, distance)); - } - } - } else - sqrDistLowerBound = distToCollision * distToCollision; - - // const Vec3f c1 = contact_point - distance * 0.5 * normal; - // const Vec3f c2 = contact_point + distance * 0.5 * normal; - internal::updateDistanceLowerBoundFromLeaf( - *crequest, *cresult, distToCollision, c1, c2, normal); - - assert(cresult->isCollision() || sqrDistLowerBound > 0); - return crequest->isSatisfied(*cresult); - } - - // Determine which tree to traverse first. - if (bvn1.isLeaf() || - (tree2->nodeHasChildren(root2) && (bv2.size() > bvn1.bv.size()))) { - for (unsigned int i = 0; i < 8; ++i) { - if (tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(bv2, i, child_bv); - - if (HeightFieldOcTreeIntersectRecurse(tree1, root1, tree2, child, - child_bv, tf1, tf2, - sqrDistLowerBound)) - return true; - } - } - } else { - if (HeightFieldOcTreeIntersectRecurse( - tree1, (unsigned int)bvn1.leftChild(), tree2, root2, bv2, tf1, - tf2, sqrDistLowerBound)) - return true; - - if (HeightFieldOcTreeIntersectRecurse( - tree1, (unsigned int)bvn1.rightChild(), tree2, root2, bv2, tf1, - tf2, sqrDistLowerBound)) - return true; - } - - return false; - } - - bool OcTreeDistanceRecurse(const OcTree* tree1, - const OcTree::OcTreeNode* root1, const AABB& bv1, - const OcTree* tree2, - const OcTree::OcTreeNode* root2, const AABB& bv2, - const Transform3f& tf1, - const Transform3f& tf2) const { - if (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) { - if (tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) { - Box box1, box2; - Transform3f box1_tf, box2_tf; - constructBox(bv1, tf1, box1, box1_tf); - constructBox(bv2, tf2, box2, box2_tf); - - if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { - box1.computeLocalAABB(); - box2.computeLocalAABB(); - } - - Vec3f p1, p2, normal; - const FCL_REAL distance = internal::ShapeShapeDistance( - &box1, box1_tf, &box2, box2_tf, this->solver, - this->drequest->enable_signed_distance, p1, p2, normal); - - this->dresult->update(distance, tree1, tree2, - (int)(root1 - tree1->getRoot()), - (int)(root2 - tree2->getRoot()), p1, p2, normal); - - return drequest->isSatisfied(*dresult); - } else - return false; - } - - if (!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) - return false; - - if (!tree2->nodeHasChildren(root2) || - (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { - for (unsigned int i = 0; i < 8; ++i) { - if (tree1->nodeChildExists(root1, i)) { - const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); - AABB child_bv; - computeChildBV(bv1, i, child_bv); - - FCL_REAL d; - AABB aabb1, aabb2; - convertBV(bv1, tf1, aabb1); - convertBV(bv2, tf2, aabb2); - d = aabb1.distance(aabb2); - - if (d < dresult->min_distance) { - if (OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, - tf1, tf2)) - return true; - } - } - } - } else { - for (unsigned int i = 0; i < 8; ++i) { - if (tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(bv2, i, child_bv); - - FCL_REAL d; - AABB aabb1, aabb2; - convertBV(bv1, tf1, aabb1); - convertBV(bv2, tf2, aabb2); - d = aabb1.distance(aabb2); - - if (d < dresult->min_distance) { - if (OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, - tf1, tf2)) - return true; - } - } - } - } - - return false; - } - - bool OcTreeIntersectRecurse(const OcTree* tree1, - const OcTree::OcTreeNode* root1, const AABB& bv1, - const OcTree* tree2, - const OcTree::OcTreeNode* root2, const AABB& bv2, - const Transform3f& tf1, - const Transform3f& tf2) const { - // Empty OcTree is considered free. - if (!root1) return false; - if (!root2) return false; - - /// stop when 1) bounding boxes of two objects not overlap; OR - /// 2) at least one of the nodes is free; OR - /// 2) (two uncertain nodes OR one node occupied and one node - /// uncertain) AND cost not required - if (tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) - return false; - else if ((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2))) - return false; - - bool bothAreLeaves = - (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)); - if (!bothAreLeaves || !crequest->enable_contact) { - OBB obb1, obb2; - convertBV(bv1, tf1, obb1); - convertBV(bv2, tf2, obb2); - FCL_REAL sqrDistLowerBound; - if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { - if (cresult->distance_lower_bound > 0 && - sqrDistLowerBound < - cresult->distance_lower_bound * cresult->distance_lower_bound) - cresult->distance_lower_bound = - sqrt(sqrDistLowerBound) - crequest->security_margin; - return false; - } - if (crequest->enable_contact) { // Overlap - if (cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact( - Contact(tree1, tree2, static_cast(root1 - tree1->getRoot()), - static_cast(root2 - tree2->getRoot()))); - return crequest->isSatisfied(*cresult); - } - } - - // Both node are leaves - if (bothAreLeaves) { - assert(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)); - - Box box1, box2; - Transform3f box1_tf, box2_tf; - constructBox(bv1, tf1, box1, box1_tf); - constructBox(bv2, tf2, box2, box2_tf); - - if (this->solver->gjk_initial_guess == - GJKInitialGuess::BoundingVolumeGuess) { - box1.computeLocalAABB(); - box2.computeLocalAABB(); - } - - // When reaching this point, `this->solver` has already been set up - // by the CollisionRequest `this->request`. - // The only thing we need to (and can) pass to `ShapeShapeDistance` is - // whether or not penetration information is should be computed in case of - // collision. - const bool compute_penetration = (this->crequest->enable_contact || - (this->crequest->security_margin < 0)); - Vec3f c1, c2, normal; - FCL_REAL distance = internal::ShapeShapeDistance( - &box1, box1_tf, &box2, box2_tf, this->solver, compute_penetration, c1, - c2, normal); - - const FCL_REAL distToCollision = - distance - this->crequest->security_margin; - - internal::updateDistanceLowerBoundFromLeaf( - *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal); - - if (this->cresult->numContacts() < this->crequest->num_max_contacts) { - if (distToCollision <= this->crequest->collision_distance_threshold) - this->cresult->addContact( - Contact(tree1, tree2, static_cast(root1 - tree1->getRoot()), - static_cast(root2 - tree2->getRoot()), c1, c2, - normal, distance)); - } - - return crequest->isSatisfied(*cresult); - } - - // Determine which tree to traverse first. - if (!tree2->nodeHasChildren(root2) || - (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { - for (unsigned int i = 0; i < 8; ++i) { - if (tree1->nodeChildExists(root1, i)) { - const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); - AABB child_bv; - computeChildBV(bv1, i, child_bv); - - if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, root2, bv2, - tf1, tf2)) - return true; - } - } - } else { - for (unsigned int i = 0; i < 8; ++i) { - if (tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(bv2, i, child_bv); - - if (OcTreeIntersectRecurse(tree1, root1, bv1, tree2, child, child_bv, - tf1, tf2)) - return true; - } - } - } - - return false; - } -}; - -/// @addtogroup Traversal_For_Collision -/// @{ - -/// @brief Traversal node for octree collision -class HPP_FCL_DLLAPI OcTreeCollisionTraversalNode - : public CollisionTraversalNodeBase { - public: - OcTreeCollisionTraversalNode(const CollisionRequest& request) - : CollisionTraversalNodeBase(request) { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - bool BVDisjoints(unsigned, unsigned, FCL_REAL&) const { return false; } - - void leafCollides(unsigned, unsigned, FCL_REAL& sqrDistLowerBound) const { - otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result); - sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); - sqrDistLowerBound *= sqrDistLowerBound; - } - - const OcTree* model1; - const OcTree* model2; - - Transform3f tf1, tf2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for shape-octree collision -template -class HPP_FCL_DLLAPI ShapeOcTreeCollisionTraversalNode - : public CollisionTraversalNodeBase { - public: - ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request) - : CollisionTraversalNodeBase(request) { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { - return false; - } - - void leafCollides(unsigned int, unsigned int, - FCL_REAL& sqrDistLowerBound) const { - otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result); - sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); - sqrDistLowerBound *= sqrDistLowerBound; - } - - const S* model1; - const OcTree* model2; - - Transform3f tf1, tf2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for octree-shape collision - -template -class HPP_FCL_DLLAPI OcTreeShapeCollisionTraversalNode - : public CollisionTraversalNodeBase { - public: - OcTreeShapeCollisionTraversalNode(const CollisionRequest& request) - : CollisionTraversalNodeBase(request) { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - bool BVDisjoints(unsigned int, unsigned int, fcl::FCL_REAL&) const { - return false; - } - - void leafCollides(unsigned int, unsigned int, - FCL_REAL& sqrDistLowerBound) const { - otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result); - sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); - sqrDistLowerBound *= sqrDistLowerBound; - } - - const OcTree* model1; - const S* model2; - - Transform3f tf1, tf2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for mesh-octree collision -template -class HPP_FCL_DLLAPI MeshOcTreeCollisionTraversalNode - : public CollisionTraversalNodeBase { - public: - MeshOcTreeCollisionTraversalNode(const CollisionRequest& request) - : CollisionTraversalNodeBase(request) { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { - return false; - } - - void leafCollides(unsigned int, unsigned int, - FCL_REAL& sqrDistLowerBound) const { - otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result); - sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); - sqrDistLowerBound *= sqrDistLowerBound; - } - - const BVHModel* model1; - const OcTree* model2; - - Transform3f tf1, tf2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for octree-mesh collision -template -class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode - : public CollisionTraversalNodeBase { - public: - OcTreeMeshCollisionTraversalNode(const CollisionRequest& request) - : CollisionTraversalNodeBase(request) { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { - return false; - } - - void leafCollides(unsigned int, unsigned int, - FCL_REAL& sqrDistLowerBound) const { - otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result); - sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); - sqrDistLowerBound *= sqrDistLowerBound; - } - - const OcTree* model1; - const BVHModel* model2; - - Transform3f tf1, tf2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for octree-height-field collision -template -class HPP_FCL_DLLAPI OcTreeHeightFieldCollisionTraversalNode - : public CollisionTraversalNodeBase { - public: - OcTreeHeightFieldCollisionTraversalNode(const CollisionRequest& request) - : CollisionTraversalNodeBase(request) { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { - return false; - } - - void leafCollides(unsigned int, unsigned int, - FCL_REAL& sqrDistLowerBound) const { - otsolver->OcTreeHeightFieldIntersect(model1, model2, tf1, tf2, request, - *result, sqrDistLowerBound); - } - - const OcTree* model1; - const HeightField* model2; - - Transform3f tf1, tf2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for octree-height-field collision -template -class HPP_FCL_DLLAPI HeightFieldOcTreeCollisionTraversalNode - : public CollisionTraversalNodeBase { - public: - HeightFieldOcTreeCollisionTraversalNode(const CollisionRequest& request) - : CollisionTraversalNodeBase(request) { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { - return false; - } - - void leafCollides(unsigned int, unsigned int, - FCL_REAL& sqrDistLowerBound) const { - otsolver->HeightFieldOcTreeIntersect(model1, model2, tf1, tf2, request, - *result, sqrDistLowerBound); - } - - const HeightField* model1; - const OcTree* model2; - - Transform3f tf1, tf2; - - const OcTreeSolver* otsolver; -}; - -/// @} - -/// @addtogroup Traversal_For_Distance -/// @{ - -/// @brief Traversal node for octree distance -class HPP_FCL_DLLAPI OcTreeDistanceTraversalNode - : public DistanceTraversalNodeBase { - public: - OcTreeDistanceTraversalNode() { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - FCL_REAL BVDistanceLowerBound(unsigned, unsigned) const { return -1; } - - bool BVDistanceLowerBound(unsigned, unsigned, FCL_REAL&) const { - return false; - } - - void leafComputeDistance(unsigned, unsigned int) const { - otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result); - } - - const OcTree* model1; - const OcTree* model2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for shape-octree distance -template -class HPP_FCL_DLLAPI ShapeOcTreeDistanceTraversalNode - : public DistanceTraversalNodeBase { - public: - ShapeOcTreeDistanceTraversalNode() { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } - - void leafComputeDistance(unsigned int, unsigned int) const { - otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result); - } - - const Shape* model1; - const OcTree* model2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for octree-shape distance -template -class HPP_FCL_DLLAPI OcTreeShapeDistanceTraversalNode - : public DistanceTraversalNodeBase { - public: - OcTreeShapeDistanceTraversalNode() { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } - - void leafComputeDistance(unsigned int, unsigned int) const { - otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result); - } - - const OcTree* model1; - const Shape* model2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for mesh-octree distance -template -class HPP_FCL_DLLAPI MeshOcTreeDistanceTraversalNode - : public DistanceTraversalNodeBase { - public: - MeshOcTreeDistanceTraversalNode() { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } - - void leafComputeDistance(unsigned int, unsigned int) const { - otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result); - } - - const BVHModel* model1; - const OcTree* model2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for octree-mesh distance -template -class HPP_FCL_DLLAPI OcTreeMeshDistanceTraversalNode - : public DistanceTraversalNodeBase { - public: - OcTreeMeshDistanceTraversalNode() { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } - - void leafComputeDistance(unsigned int, unsigned int) const { - otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result); - } - - const OcTree* model1; - const BVHModel* model2; - - const OcTreeSolver* otsolver; -}; - -/// @} - -} // namespace fcl - -} // namespace hpp - -/// @endcond - -#endif +#include +#include diff --git a/include/hpp/fcl/internal/traversal_node_setup.h b/include/hpp/fcl/internal/traversal_node_setup.h index 20b5c71db..b94dd7a84 100644 --- a/include/hpp/fcl/internal/traversal_node_setup.h +++ b/include/hpp/fcl/internal/traversal_node_setup.h @@ -1,817 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_TRAVERSAL_NODE_SETUP_H -#define HPP_FCL_TRAVERSAL_NODE_SETUP_H - -/// @cond INTERNAL - -#include -#include - -#include -#include - -// #include -#include - -#ifdef HPP_FCL_HAS_OCTOMAP -#include -#endif - -#include - -namespace hpp { -namespace fcl { - -#ifdef HPP_FCL_HAS_OCTOMAP -/// @brief Initialize traversal node for collision between two octrees, given -/// current object transform -inline bool initialize(OcTreeCollisionTraversalNode& node, const OcTree& model1, - const Transform3f& tf1, const OcTree& model2, - const Transform3f& tf2, const OcTreeSolver* otsolver, - CollisionResult& result) { - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for distance between two octrees, given -/// current object transform -inline bool initialize(OcTreeDistanceTraversalNode& node, const OcTree& model1, - const Transform3f& tf1, const OcTree& model2, - const Transform3f& tf2, const OcTreeSolver* otsolver, - const DistanceRequest& request, DistanceResult& result) - -{ - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for collision between one shape and one -/// octree, given current object transform -template -bool initialize(ShapeOcTreeCollisionTraversalNode& node, const S& model1, - const Transform3f& tf1, const OcTree& model2, - const Transform3f& tf2, const OcTreeSolver* otsolver, - CollisionResult& result) { - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for collision between one octree and one -/// shape, given current object transform -template -bool initialize(OcTreeShapeCollisionTraversalNode& node, - const OcTree& model1, const Transform3f& tf1, const S& model2, - const Transform3f& tf2, const OcTreeSolver* otsolver, - CollisionResult& result) { - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for distance between one shape and one -/// octree, given current object transform -template -bool initialize(ShapeOcTreeDistanceTraversalNode& node, const S& model1, - const Transform3f& tf1, const OcTree& model2, - const Transform3f& tf2, const OcTreeSolver* otsolver, - const DistanceRequest& request, DistanceResult& result) { - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for distance between one octree and one -/// shape, given current object transform -template -bool initialize(OcTreeShapeDistanceTraversalNode& node, const OcTree& model1, - const Transform3f& tf1, const S& model2, const Transform3f& tf2, - const OcTreeSolver* otsolver, const DistanceRequest& request, - DistanceResult& result) { - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for collision between one mesh and one -/// octree, given current object transform -template -bool initialize(MeshOcTreeCollisionTraversalNode& node, - const BVHModel& model1, const Transform3f& tf1, - const OcTree& model2, const Transform3f& tf2, - const OcTreeSolver* otsolver, CollisionResult& result) { - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for collision between one octree and one -/// mesh, given current object transform -template -bool initialize(OcTreeMeshCollisionTraversalNode& node, - const OcTree& model1, const Transform3f& tf1, - const BVHModel& model2, const Transform3f& tf2, - const OcTreeSolver* otsolver, CollisionResult& result) { - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for collision between one octree and one -/// height field, given current object transform -template -bool initialize(OcTreeHeightFieldCollisionTraversalNode& node, - const OcTree& model1, const Transform3f& tf1, - const HeightField& model2, const Transform3f& tf2, - const OcTreeSolver* otsolver, CollisionResult& result) { - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for collision between one height field and -/// one octree, given current object transform -template -bool initialize(HeightFieldOcTreeCollisionTraversalNode& node, - const HeightField& model1, const Transform3f& tf1, - const OcTree& model2, const Transform3f& tf2, - const OcTreeSolver* otsolver, CollisionResult& result) { - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for distance between one mesh and one -/// octree, given current object transform -template -bool initialize(MeshOcTreeDistanceTraversalNode& node, - const BVHModel& model1, const Transform3f& tf1, - const OcTree& model2, const Transform3f& tf2, - const OcTreeSolver* otsolver, const DistanceRequest& request, - DistanceResult& result) { - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for collision between one octree and one -/// mesh, given current object transform -template -bool initialize(OcTreeMeshDistanceTraversalNode& node, const OcTree& model1, - const Transform3f& tf1, const BVHModel& model2, - const Transform3f& tf2, const OcTreeSolver* otsolver, - const DistanceRequest& request, DistanceResult& result) { - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -#endif - -/// @brief Initialize traversal node for collision between two geometric shapes, -/// given current object transform -template -bool initialize(ShapeCollisionTraversalNode& node, const S1& shape1, - const Transform3f& tf1, const S2& shape2, - const Transform3f& tf2, const GJKSolver* nsolver, - CollisionResult& result) { - node.model1 = &shape1; - node.tf1 = tf1; - node.model2 = &shape2; - node.tf2 = tf2; - node.nsolver = nsolver; - - node.result = &result; - - return true; -} - -/// @brief Initialize traversal node for collision between one mesh and one -/// shape, given current object transform -template -bool initialize(MeshShapeCollisionTraversalNode& node, - BVHModel& model1, Transform3f& tf1, const S& model2, - const Transform3f& tf2, const GJKSolver* nsolver, - CollisionResult& result, bool use_refit = false, - bool refit_bottomup = false) { - if (model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( - "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", - std::invalid_argument) - - if (!tf1.isIdentity() && - model1.vertices.get()) // TODO(jcarpent): vectorized version - { - std::vector vertices_transformed(model1.num_vertices); - const std::vector& model1_vertices_ = *(model1.vertices); - for (unsigned int i = 0; i < model1.num_vertices; ++i) { - const Vec3f& p = model1_vertices_[i]; - Vec3f new_v = tf1.transform(p); - vertices_transformed[i] = new_v; - } - - model1.beginReplaceModel(); - model1.replaceSubModel(vertices_transformed); - model1.endReplaceModel(use_refit, refit_bottomup); - - tf1.setIdentity(); - } - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - computeBV(model2, tf2, node.model2_bv); - - node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL; - node.tri_indices = - model1.tri_indices.get() ? model1.tri_indices->data() : NULL; - - node.result = &result; - - return true; -} - -/// @brief Initialize traversal node for collision between one mesh and one -/// shape -template -bool initialize(MeshShapeCollisionTraversalNode& node, - const BVHModel& model1, const Transform3f& tf1, - const S& model2, const Transform3f& tf2, - const GJKSolver* nsolver, CollisionResult& result) { - if (model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( - "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", - std::invalid_argument) - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - computeBV(model2, tf2, node.model2_bv); - - node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL; - node.tri_indices = - model1.tri_indices.get() ? model1.tri_indices->data() : NULL; - - node.result = &result; - - return true; -} - -/// @brief Initialize traversal node for collision between one mesh and one -/// shape, given current object transform -template -bool initialize(HeightFieldShapeCollisionTraversalNode& node, - HeightField& model1, Transform3f& tf1, const S& model2, - const Transform3f& tf2, const GJKSolver* nsolver, - CollisionResult& result, bool use_refit = false, - bool refit_bottomup = false); - -/// @brief Initialize traversal node for collision between one mesh and one -/// shape -template -bool initialize(HeightFieldShapeCollisionTraversalNode& node, - const HeightField& model1, const Transform3f& tf1, - const S& model2, const Transform3f& tf2, - const GJKSolver* nsolver, CollisionResult& result) { - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - computeBV(model2, tf2, node.model2_bv); - - node.result = &result; - - return true; -} - -/// @cond IGNORE -namespace details { -template class OrientedNode> -static inline bool setupShapeMeshCollisionOrientedNode( - OrientedNode& node, const S& model1, const Transform3f& tf1, - const BVHModel& model2, const Transform3f& tf2, - const GJKSolver* nsolver, CollisionResult& result) { - if (model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( - "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", - std::invalid_argument) - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - computeBV(model1, tf1, node.model1_bv); - - node.vertices = model2.vertices.get() ? model2.vertices->data() : NULL; - node.tri_indices = - model2.tri_indices.get() ? model2.tri_indices->data() : NULL; - - node.result = &result; - - return true; -} -} // namespace details -/// @endcond - -/// @brief Initialize traversal node for collision between two meshes, given the -/// current transforms -template -bool initialize( - MeshCollisionTraversalNode& node, - BVHModel& model1, Transform3f& tf1, BVHModel& model2, - Transform3f& tf2, CollisionResult& result, bool use_refit = false, - bool refit_bottomup = false) { - if (model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( - "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", - std::invalid_argument) - if (model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( - "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", - std::invalid_argument) - - if (!tf1.isIdentity() && model1.vertices.get()) { - std::vector vertices_transformed1(model1.num_vertices); - const std::vector& model1_vertices_ = *(model1.vertices); - for (unsigned int i = 0; i < model1.num_vertices; ++i) { - const Vec3f& p = model1_vertices_[i]; - Vec3f new_v = tf1.transform(p); - vertices_transformed1[i] = new_v; - } - - model1.beginReplaceModel(); - model1.replaceSubModel(vertices_transformed1); - model1.endReplaceModel(use_refit, refit_bottomup); - - tf1.setIdentity(); - } - - if (!tf2.isIdentity() && model2.vertices.get()) { - std::vector vertices_transformed2(model2.num_vertices); - const std::vector& model2_vertices_ = *(model2.vertices); - for (unsigned int i = 0; i < model2.num_vertices; ++i) { - const Vec3f& p = model2_vertices_[i]; - Vec3f new_v = tf2.transform(p); - vertices_transformed2[i] = new_v; - } - - model2.beginReplaceModel(); - model2.replaceSubModel(vertices_transformed2); - model2.endReplaceModel(use_refit, refit_bottomup); - - tf2.setIdentity(); - } - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - - node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL; - node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL; - - node.tri_indices1 = - model1.tri_indices.get() ? model1.tri_indices->data() : NULL; - node.tri_indices2 = - model2.tri_indices.get() ? model2.tri_indices->data() : NULL; - - node.result = &result; - - return true; -} - -template -bool initialize(MeshCollisionTraversalNode& node, - const BVHModel& model1, const Transform3f& tf1, - const BVHModel& model2, const Transform3f& tf2, - CollisionResult& result) { - if (model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( - "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", - std::invalid_argument) - if (model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( - "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", - std::invalid_argument) - - node.vertices1 = model1.vertices ? model1.vertices->data() : NULL; - node.vertices2 = model2.vertices ? model2.vertices->data() : NULL; - - node.tri_indices1 = - model1.tri_indices.get() ? model1.tri_indices->data() : NULL; - node.tri_indices2 = - model2.tri_indices.get() ? model2.tri_indices->data() : NULL; - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - - node.result = &result; - - node.RT.R.noalias() = tf1.getRotation().transpose() * tf2.getRotation(); - node.RT.T.noalias() = tf1.getRotation().transpose() * - (tf2.getTranslation() - tf1.getTranslation()); - - return true; -} - -/// @brief Initialize traversal node for distance between two geometric shapes -template -bool initialize(ShapeDistanceTraversalNode& node, const S1& shape1, - const Transform3f& tf1, const S2& shape2, - const Transform3f& tf2, const GJKSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) { - node.request = request; - node.result = &result; - - node.model1 = &shape1; - node.tf1 = tf1; - node.model2 = &shape2; - node.tf2 = tf2; - node.nsolver = nsolver; - - return true; -} - -/// @brief Initialize traversal node for distance computation between two -/// meshes, given the current transforms -template -bool initialize( - MeshDistanceTraversalNode& node, - BVHModel& model1, Transform3f& tf1, BVHModel& model2, - Transform3f& tf2, const DistanceRequest& request, DistanceResult& result, - bool use_refit = false, bool refit_bottomup = false) { - if (model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( - "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", - std::invalid_argument) - if (model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( - "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", - std::invalid_argument) - - if (!tf1.isIdentity() && model1.vertices.get()) { - std::vector vertices_transformed1(model1.num_vertices); - const std::vector& model1_vertices_ = *(model1.vertices); - for (unsigned int i = 0; i < model1.num_vertices; ++i) { - const Vec3f& p = model1_vertices_[i]; - Vec3f new_v = tf1.transform(p); - vertices_transformed1[i] = new_v; - } - - model1.beginReplaceModel(); - model1.replaceSubModel(vertices_transformed1); - model1.endReplaceModel(use_refit, refit_bottomup); - - tf1.setIdentity(); - } - - if (!tf2.isIdentity() && model2.vertices.get()) { - std::vector vertices_transformed2(model2.num_vertices); - const std::vector& model2_vertices_ = *(model2.vertices); - for (unsigned int i = 0; i < model2.num_vertices; ++i) { - const Vec3f& p = model2_vertices_[i]; - Vec3f new_v = tf2.transform(p); - vertices_transformed2[i] = new_v; - } - - model2.beginReplaceModel(); - model2.replaceSubModel(vertices_transformed2); - model2.endReplaceModel(use_refit, refit_bottomup); - - tf2.setIdentity(); - } - - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - - node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL; - node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL; - - node.tri_indices1 = - model1.tri_indices.get() ? model1.tri_indices->data() : NULL; - node.tri_indices2 = - model2.tri_indices.get() ? model2.tri_indices->data() : NULL; - - return true; -} - -/// @brief Initialize traversal node for distance computation between two meshes -template -bool initialize(MeshDistanceTraversalNode& node, - const BVHModel& model1, const Transform3f& tf1, - const BVHModel& model2, const Transform3f& tf2, - const DistanceRequest& request, DistanceResult& result) { - if (model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( - "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", - std::invalid_argument) - if (model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( - "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", - std::invalid_argument) - - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - - node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL; - node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL; - - node.tri_indices1 = - model1.tri_indices.get() ? model1.tri_indices->data() : NULL; - node.tri_indices2 = - model2.tri_indices.get() ? model2.tri_indices->data() : NULL; - - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED - - relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), - tf2.getTranslation(), node.RT.R, node.RT.T); - - HPP_FCL_COMPILER_DIAGNOSTIC_POP - - return true; -} - -/// @brief Initialize traversal node for distance computation between one mesh -/// and one shape, given the current transforms -template -bool initialize(MeshShapeDistanceTraversalNode& node, - BVHModel& model1, Transform3f& tf1, const S& model2, - const Transform3f& tf2, const GJKSolver* nsolver, - const DistanceRequest& request, DistanceResult& result, - bool use_refit = false, bool refit_bottomup = false) { - if (model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( - "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", - std::invalid_argument) - - if (!tf1.isIdentity() && model1.vertices.get()) { - const std::vector& model1_vertices_ = *(model1.vertices); - std::vector vertices_transformed1(model1.num_vertices); - for (unsigned int i = 0; i < model1.num_vertices; ++i) { - const Vec3f& p = model1_vertices_[i]; - Vec3f new_v = tf1.transform(p); - vertices_transformed1[i] = new_v; - } - - model1.beginReplaceModel(); - model1.replaceSubModel(vertices_transformed1); - model1.endReplaceModel(use_refit, refit_bottomup); - - tf1.setIdentity(); - } - - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL; - node.tri_indices = - model1.tri_indices.get() ? model1.tri_indices->data() : NULL; - - computeBV(model2, tf2, node.model2_bv); - - return true; -} - -/// @cond IGNORE -namespace details { - -template class OrientedNode> -static inline bool setupMeshShapeDistanceOrientedNode( - OrientedNode& node, const BVHModel& model1, const Transform3f& tf1, - const S& model2, const Transform3f& tf2, const GJKSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) { - if (model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( - "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", - std::invalid_argument) - - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - computeBV(model2, tf2, node.model2_bv); - - node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL; - node.tri_indices = - model1.tri_indices.get() ? model1.tri_indices->data() : NULL; - - return true; -} -} // namespace details -/// @endcond - -/// @brief Initialize traversal node for distance computation between one mesh -/// and one shape, specialized for RSS type -template -bool initialize(MeshShapeDistanceTraversalNodeRSS& node, - const BVHModel& model1, const Transform3f& tf1, - const S& model2, const Transform3f& tf2, - const GJKSolver* nsolver, const DistanceRequest& request, - DistanceResult& result) { - return details::setupMeshShapeDistanceOrientedNode( - node, model1, tf1, model2, tf2, nsolver, request, result); -} - -/// @brief Initialize traversal node for distance computation between one mesh -/// and one shape, specialized for kIOS type -template -bool initialize(MeshShapeDistanceTraversalNodekIOS& node, - const BVHModel& model1, const Transform3f& tf1, - const S& model2, const Transform3f& tf2, - const GJKSolver* nsolver, const DistanceRequest& request, - DistanceResult& result) { - return details::setupMeshShapeDistanceOrientedNode( - node, model1, tf1, model2, tf2, nsolver, request, result); -} - -/// @brief Initialize traversal node for distance computation between one mesh -/// and one shape, specialized for OBBRSS type -template -bool initialize(MeshShapeDistanceTraversalNodeOBBRSS& node, - const BVHModel& model1, const Transform3f& tf1, - const S& model2, const Transform3f& tf2, - const GJKSolver* nsolver, const DistanceRequest& request, - DistanceResult& result) { - return details::setupMeshShapeDistanceOrientedNode( - node, model1, tf1, model2, tf2, nsolver, request, result); -} - -} // namespace fcl -} // namespace hpp - -/// @endcond - -#endif +#include +#include diff --git a/include/hpp/fcl/internal/traversal_node_shapes.h b/include/hpp/fcl/internal/traversal_node_shapes.h index e30db3cb6..996730308 100644 --- a/include/hpp/fcl/internal/traversal_node_shapes.h +++ b/include/hpp/fcl/internal/traversal_node_shapes.h @@ -1,127 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_TRAVERSAL_NODE_SHAPES_H -#define HPP_FCL_TRAVERSAL_NODE_SHAPES_H - -/// @cond INTERNAL - -#include "hpp/fcl/collision_data.h" -#include "hpp/fcl/BV/BV.h" -#include "hpp/fcl/shape/geometric_shapes_utility.h" -#include "hpp/fcl/internal/traversal_node_base.h" -#include "hpp/fcl/internal/shape_shape_func.h" - -namespace hpp { -namespace fcl { - -/// @addtogroup Traversal_For_Collision -/// @{ - -/// @brief Traversal node for collision between two shapes -template -class HPP_FCL_DLLAPI ShapeCollisionTraversalNode - : public CollisionTraversalNodeBase { - public: - ShapeCollisionTraversalNode(const CollisionRequest& request) - : CollisionTraversalNodeBase(request) { - model1 = NULL; - model2 = NULL; - - nsolver = NULL; - } - - /// @brief BV culling test in one BVTT node - bool BVDisjoints(int, int, FCL_REAL&) const { - HPP_FCL_THROW_PRETTY("Not implemented", std::runtime_error); - } - - /// @brief Intersection testing between leaves (two shapes) - void leafCollides(int, int, FCL_REAL&) const { - ShapeShapeCollide(this->model1, this->tf1, this->model2, this->tf2, - this->nsolver, this->request, *(this->result)); - } - - const S1* model1; - const S2* model2; - - const GJKSolver* nsolver; -}; - -/// @} - -/// @addtogroup Traversal_For_Distance -/// @{ - -/// @brief Traversal node for distance between two shapes -template -class HPP_FCL_DLLAPI ShapeDistanceTraversalNode - : public DistanceTraversalNodeBase { - public: - ShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { - model1 = NULL; - model2 = NULL; - - nsolver = NULL; - } - - /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { - return -1; // should not be used - } - - /// @brief Distance testing between leaves (two shapes) - void leafComputeDistance(unsigned int, unsigned int) const { - ShapeShapeDistance(this->model1, this->tf1, this->model2, this->tf2, - this->nsolver, this->request, *(this->result)); - } - - const S1* model1; - const S2* model2; - - const GJKSolver* nsolver; -}; - -/// @} - -} // namespace fcl - -} // namespace hpp - -/// @endcond - -#endif +#include +#include diff --git a/include/hpp/fcl/internal/traversal_recurse.h b/include/hpp/fcl/internal/traversal_recurse.h index 1f15fe4f2..6fd2bf7ae 100644 --- a/include/hpp/fcl/internal/traversal_recurse.h +++ b/include/hpp/fcl/internal/traversal_recurse.h @@ -1,83 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_TRAVERSAL_RECURSE_H -#define HPP_FCL_TRAVERSAL_RECURSE_H - -/// @cond INTERNAL - -#include -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// Recurse function for collision -/// @param node collision node, -/// @param b1, b2 ids of bounding volume nodes for object 1 and object 2 -/// @retval sqrDistLowerBound squared lower bound on distance between objects. -void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1, - unsigned int b2, BVHFrontList* front_list, - FCL_REAL& sqrDistLowerBound); - -void collisionNonRecurse(CollisionTraversalNodeBase* node, - BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound); - -/// @brief Recurse function for distance -void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1, - unsigned int b2, BVHFrontList* front_list); - -/// @brief Recurse function for distance, using queue acceleration -void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1, - unsigned int b2, BVHFrontList* front_list, - unsigned int qsize); - -/// @brief Recurse function for front list propagation -void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, - const CollisionRequest& request, - CollisionResult& result, - BVHFrontList* front_list); - -} // namespace fcl - -} // namespace hpp - -/// @endcond - -#endif +#include +#include diff --git a/include/hpp/fcl/logging.h b/include/hpp/fcl/logging.h index c65624b3d..e9f591ebc 100644 --- a/include/hpp/fcl/logging.h +++ b/include/hpp/fcl/logging.h @@ -1,54 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2024, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/// This file defines basic logging macros for HPP-FCL, based on Boost.Log. -/// To enable logging, define the preprocessor macro `HPP_FCL_ENABLE_LOGGING`. - -#ifndef HPP_FCL_LOGGING_H -#define HPP_FCL_LOGGING_H - -#ifdef HPP_FCL_ENABLE_LOGGING -#include -#define HPP_FCL_LOG_INFO(message) BOOST_LOG_TRIVIAL(info) << message -#define HPP_FCL_LOG_DEBUG(message) BOOST_LOG_TRIVIAL(debug) << message -#define HPP_FCL_LOG_WARNING(message) BOOST_LOG_TRIVIAL(warning) << message -#define HPP_FCL_LOG_ERROR(message) BOOST_LOG_TRIVIAL(error) << message -#else -#define HPP_FCL_LOG_INFO(message) -#define HPP_FCL_LOG_DEBUG(message) -#define HPP_FCL_LOG_WARNING(message) -#define HPP_FCL_LOG_ERROR(message) -#endif - -#endif // HPP_FCL_LOGGING_H +#include +#include diff --git a/include/hpp/fcl/math/matrix_3f.h b/include/hpp/fcl/math/matrix_3f.h index 9c1bdb0b5..0b4e853e9 100644 --- a/include/hpp/fcl/math/matrix_3f.h +++ b/include/hpp/fcl/math/matrix_3f.h @@ -1,46 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_MATRIX_3F_H -#define HPP_FCL_MATRIX_3F_H - -#warning "This file is deprecated. Include instead." - -// List of equivalent includes. -#include - -#endif +#include +#include diff --git a/include/hpp/fcl/math/transform.h b/include/hpp/fcl/math/transform.h index 9a1d31e5d..970a2a936 100644 --- a/include/hpp/fcl/math/transform.h +++ b/include/hpp/fcl/math/transform.h @@ -1,272 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_TRANSFORM_H -#define HPP_FCL_TRANSFORM_H - -#include "hpp/fcl/fwd.hh" -#include "hpp/fcl/data_types.h" - -namespace hpp { -namespace fcl { - -HPP_FCL_DEPRECATED typedef Eigen::Quaternion Quaternion3f; -typedef Eigen::Quaternion Quatf; - -static inline std::ostream& operator<<(std::ostream& o, const Quatf& q) { - o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")"; - return o; -} - -/// @brief Simple transform class used locally by InterpMotion -class HPP_FCL_DLLAPI Transform3f { - /// @brief Matrix cache - Matrix3f R; - - /// @brief Translation vector - Vec3f T; - - public: - /// @brief Default transform is no movement - Transform3f() { - setIdentity(); // set matrix_set true - } - - static Transform3f Identity() { return Transform3f(); } - - /// @brief Construct transform from rotation and translation - template - Transform3f(const Eigen::MatrixBase& R_, - const Eigen::MatrixBase& T_) - : R(R_), T(T_) {} - - /// @brief Construct transform from rotation and translation - template - Transform3f(const Quatf& q_, const Eigen::MatrixBase& T_) - : R(q_.toRotationMatrix()), T(T_) {} - - /// @brief Construct transform from rotation - Transform3f(const Matrix3f& R_) : R(R_), T(Vec3f::Zero()) {} - - /// @brief Construct transform from rotation - Transform3f(const Quatf& q_) : R(q_), T(Vec3f::Zero()) {} - - /// @brief Construct transform from translation - Transform3f(const Vec3f& T_) : R(Matrix3f::Identity()), T(T_) {} - - /// @brief Construct transform from other transform - Transform3f(const Transform3f& tf) : R(tf.R), T(tf.T) {} - - /// @brief operator = - Transform3f& operator=(const Transform3f& tf) { - R = tf.R; - T = tf.T; - return *this; - } - - /// @brief get translation - inline const Vec3f& getTranslation() const { return T; } - - /// @brief get translation - inline const Vec3f& translation() const { return T; } - - /// @brief get translation - inline Vec3f& translation() { return T; } - - /// @brief get rotation - inline const Matrix3f& getRotation() const { return R; } - - /// @brief get rotation - inline const Matrix3f& rotation() const { return R; } - - /// @brief get rotation - inline Matrix3f& rotation() { return R; } - - /// @brief get quaternion - inline Quatf getQuatRotation() const { return Quatf(R); } - - /// @brief set transform from rotation and translation - template - inline void setTransform(const Eigen::MatrixBase& R_, - const Eigen::MatrixBase& T_) { - R.noalias() = R_; - T.noalias() = T_; - } - - /// @brief set transform from rotation and translation - inline void setTransform(const Quatf& q_, const Vec3f& T_) { - R = q_.toRotationMatrix(); - T = T_; - } - - /// @brief set transform from rotation - template - inline void setRotation(const Eigen::MatrixBase& R_) { - R.noalias() = R_; - } - - /// @brief set transform from translation - template - inline void setTranslation(const Eigen::MatrixBase& T_) { - T.noalias() = T_; - } - - /// @brief set transform from rotation - inline void setQuatRotation(const Quatf& q_) { R = q_.toRotationMatrix(); } - - /// @brief transform a given vector by the transform - template - inline Vec3f transform(const Eigen::MatrixBase& v) const { - return R * v + T; - } - - /// @brief transform a given vector by the inverse of the transform - template - inline Vec3f inverseTransform(const Eigen::MatrixBase& v) const { - return R.transpose() * (v - T); - } - - /// @brief inverse transform - inline Transform3f& inverseInPlace() { - R.transposeInPlace(); - T = -R * T; - return *this; - } - - /// @brief inverse transform - inline Transform3f inverse() { - return Transform3f(R.transpose(), -R.transpose() * T); - } - - /// @brief inverse the transform and multiply with another - inline Transform3f inverseTimes(const Transform3f& other) const { - return Transform3f(R.transpose() * other.R, R.transpose() * (other.T - T)); - } - - /// @brief multiply with another transform - inline const Transform3f& operator*=(const Transform3f& other) { - T += R * other.T; - R *= other.R; - return *this; - } - - /// @brief multiply with another transform - inline Transform3f operator*(const Transform3f& other) const { - return Transform3f(R * other.R, R * other.T + T); - } - - /// @brief check whether the transform is identity - inline bool isIdentity( - const FCL_REAL& prec = - Eigen::NumTraits::dummy_precision()) const { - return R.isIdentity(prec) && T.isZero(prec); - } - - /// @brief set the transform to be identity transform - inline void setIdentity() { - R.setIdentity(); - T.setZero(); - } - - /// @brief return a random transform - static Transform3f Random() { return Transform3f().setRandom(); } - - /// @brief set the transform to a random transform - Transform3f& setRandom(); - - bool operator==(const Transform3f& other) const { - return (R == other.getRotation()) && (T == other.getTranslation()); - } - - bool operator!=(const Transform3f& other) const { return !(*this == other); } - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -template -inline Quatf fromAxisAngle(const Eigen::MatrixBase& axis, - FCL_REAL angle) { - return Quatf(Eigen::AngleAxis(angle, axis)); -} - -/// @brief Uniformly random quaternion sphere. -/// Code taken from Pinocchio (https://github.com/stack-of-tasks/pinocchio). -inline Quatf uniformRandomQuaternion() { - // Rotational part - const FCL_REAL u1 = (FCL_REAL)rand() / RAND_MAX; - const FCL_REAL u2 = (FCL_REAL)rand() / RAND_MAX; - const FCL_REAL u3 = (FCL_REAL)rand() / RAND_MAX; - - const FCL_REAL mult1 = std::sqrt(FCL_REAL(1.0) - u1); - const FCL_REAL mult2 = std::sqrt(u1); - - static const FCL_REAL PI_value = static_cast(EIGEN_PI); - FCL_REAL s2 = std::sin(2 * PI_value * u2); - FCL_REAL c2 = std::cos(2 * PI_value * u2); - FCL_REAL s3 = std::sin(2 * PI_value * u3); - FCL_REAL c3 = std::cos(2 * PI_value * u3); - - Quatf q; - q.w() = mult1 * s2; - q.x() = mult1 * c2; - q.y() = mult2 * s3; - q.z() = mult2 * c3; - return q; -} - -inline Transform3f& Transform3f::setRandom() { - const Quatf q = uniformRandomQuaternion(); - this->rotation() = q.matrix(); - this->translation().setRandom(); - - return *this; -} - -/// @brief Construct othonormal basis from vector. -/// The z-axis is the normalized input vector. -inline Matrix3f constructOrthonormalBasisFromVector(const Vec3f& vec) { - Matrix3f basis = Matrix3f::Zero(); - basis.col(2) = vec.normalized(); - basis.col(1) = -vec.unitOrthogonal(); - basis.col(0) = basis.col(1).cross(vec); - return basis; -} - -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/math/types.h b/include/hpp/fcl/math/types.h index ece985db8..932a7e590 100644 --- a/include/hpp/fcl/math/types.h +++ b/include/hpp/fcl/math/types.h @@ -1,46 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Joseph Mirabel */ - -#ifndef HPP_FCL_MATH_TYPES_H -#define HPP_FCL_MATH_TYPES_H - -#warning "This file is deprecated. Include instead." - -// List of equivalent includes. -#include - -#endif \ No newline at end of file +#include +#include diff --git a/include/hpp/fcl/math/vec_3f.h b/include/hpp/fcl/math/vec_3f.h index 4389bba75..e6ac46c7a 100644 --- a/include/hpp/fcl/math/vec_3f.h +++ b/include/hpp/fcl/math/vec_3f.h @@ -1,46 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_VEC_3F_H -#define HPP_FCL_VEC_3F_H - -#warning "This file is deprecated. Include instead." - -// List of equivalent includes. -#include - -#endif +#include +#include diff --git a/include/hpp/fcl/mesh_loader/assimp.h b/include/hpp/fcl/mesh_loader/assimp.h index 115933740..139538928 100644 --- a/include/hpp/fcl/mesh_loader/assimp.h +++ b/include/hpp/fcl/mesh_loader/assimp.h @@ -1,130 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * Copyright (c) 2016-2019, CNRS - LAAS - * Copyright (c) 2019, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef HPP_FCL_MESH_LOADER_ASSIMP_H -#define HPP_FCL_MESH_LOADER_ASSIMP_H - -#include -#include -#include -#include - -struct aiScene; -namespace Assimp { -class Importer; -} - -namespace hpp { -namespace fcl { - -namespace internal { - -struct HPP_FCL_DLLAPI TriangleAndVertices { - std::vector vertices_; - std::vector triangles_; -}; - -struct HPP_FCL_DLLAPI Loader { - Loader(); - ~Loader(); - - void load(const std::string& resource_path); - - Assimp::Importer* importer; - aiScene const* scene; -}; - -/** - * @brief Recursive procedure for building a mesh - * - * @param[in] scale Scale to apply when reading the ressource - * @param[in] scene Pointer to the assimp scene - * @param[in] vertices_offset Current number of vertices in the model - * @param tv Triangles and Vertices of the mesh submodels - */ -HPP_FCL_DLLAPI void buildMesh(const fcl::Vec3f& scale, const aiScene* scene, - unsigned vertices_offset, - TriangleAndVertices& tv); - -/** - * @brief Convert an assimp scene to a mesh - * - * @param[in] scale Scale to apply when reading the ressource - * @param[in] scene Pointer to the assimp scene - * @param[out] mesh The mesh that must be built - */ -template -inline void meshFromAssimpScene( - const fcl::Vec3f& scale, const aiScene* scene, - const shared_ptr >& mesh) { - TriangleAndVertices tv; - - int res = mesh->beginModel(); - - if (res != fcl::BVH_OK) { - HPP_FCL_THROW_PRETTY("fcl BVHReturnCode = " << res, std::runtime_error); - } - - buildMesh(scale, scene, (unsigned)mesh->num_vertices, tv); - mesh->addSubModel(tv.vertices_, tv.triangles_); - - mesh->endModel(); -} - -} // namespace internal - -/** - * @brief Read a mesh file and convert it to a polyhedral mesh - * - * @param[in] resource_path Path to the ressource mesh file to be read - * @param[in] scale Scale to apply when reading the ressource - * @param[out] polyhedron The resulted polyhedron - */ -template -inline void loadPolyhedronFromResource( - const std::string& resource_path, const fcl::Vec3f& scale, - const shared_ptr >& polyhedron) { - internal::Loader scene; - scene.load(resource_path); - - internal::meshFromAssimpScene(scale, scene.scene, polyhedron); -} - -} // namespace fcl -} // namespace hpp - -#endif // HPP_FCL_MESH_LOADER_ASSIMP_H +#include +#include diff --git a/include/hpp/fcl/mesh_loader/loader.h b/include/hpp/fcl/mesh_loader/loader.h index 2c3f36249..bbb43f1d0 100644 --- a/include/hpp/fcl/mesh_loader/loader.h +++ b/include/hpp/fcl/mesh_loader/loader.h @@ -1,105 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * Copyright (c) 2016, CNRS - LAAS - * Copyright (c) 2020, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef HPP_FCL_MESH_LOADER_LOADER_H -#define HPP_FCL_MESH_LOADER_LOADER_H - -#include -#include -#include -#include - -#include -#include - -namespace hpp { -namespace fcl { -/// Base class for building polyhedron from files. -/// This class builds a new object for each file. -class HPP_FCL_DLLAPI MeshLoader { - public: - virtual ~MeshLoader() {} - - virtual BVHModelPtr_t load(const std::string& filename, - const Vec3f& scale = Vec3f::Ones()); - - /// Create an OcTree from a file in binary octomap format. - /// \todo add OctreePtr_t - virtual CollisionGeometryPtr_t loadOctree(const std::string& filename); - - MeshLoader(const NODE_TYPE& bvType = BV_OBBRSS) : bvType_(bvType) {} - - private: - const NODE_TYPE bvType_; -}; - -/// Class for building polyhedron from files with cache mechanism. -/// This class builds a new object for each different file. -/// If method CachedMeshLoader::load is called twice with the same arguments, -/// the second call returns the result of the first call. -class HPP_FCL_DLLAPI CachedMeshLoader : public MeshLoader { - public: - virtual ~CachedMeshLoader() {} - - CachedMeshLoader(const NODE_TYPE& bvType = BV_OBBRSS) : MeshLoader(bvType) {} - - virtual BVHModelPtr_t load(const std::string& filename, const Vec3f& scale); - - struct HPP_FCL_DLLAPI Key { - std::string filename; - Vec3f scale; - - Key(const std::string& f, const Vec3f& s) : filename(f), scale(s) {} - - bool operator<(const CachedMeshLoader::Key& b) const; - }; - struct HPP_FCL_DLLAPI Value { - BVHModelPtr_t model; - std::time_t mtime; - }; - typedef std::map Cache_t; - - const Cache_t& cache() const { return cache_; } - - private: - Cache_t cache_; -}; -} // namespace fcl - -} // namespace hpp - -#endif // FCL_MESH_LOADER_LOADER_H +#include +#include diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index 206109ef8..cab294b93 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -1,457 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * Copyright (c) 2021-2024, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_GJK_H -#define HPP_FCL_GJK_H - -#include - -#include "hpp/fcl/narrowphase/minkowski_difference.h" - -namespace hpp { -namespace fcl { - -namespace details { - -/// @brief class for GJK algorithm -/// -/// @note The computations are performed in the frame of the first shape. -struct HPP_FCL_DLLAPI GJK { - struct HPP_FCL_DLLAPI SimplexV { - /// @brief support vector for shape 0 and 1. - Vec3f w0, w1; - /// @brief support vector (i.e., the furthest point on the shape along the - /// support direction) - Vec3f w; - }; - - typedef unsigned char vertex_id_t; - - /// @brief A simplex is a set of up to 4 vertices. - /// Its rank is the number of vertices it contains. - /// @note This data structure does **not** own the vertices it refers to. - /// To be efficient, the constructor of `GJK` creates storage for 4 vertices. - /// Since GJK does not need any more storage, it reuses these vertices - /// throughout the algorithm by using multiple instance of this `Simplex` - /// class. - struct HPP_FCL_DLLAPI Simplex { - /// @brief simplex vertex - SimplexV* vertex[4]; - /// @brief size of simplex (number of vertices) - vertex_id_t rank; - - Simplex() {} - - void reset() { - rank = 0; - for (size_t i = 0; i < 4; ++i) vertex[i] = nullptr; - } - }; - - /// @brief Status of the GJK algorithm: - /// DidNotRun: GJK has not been run. - /// Failed: GJK did not converge (it exceeded the maximum number of - /// iterations). - /// NoCollisionEarlyStopped: GJK found a separating hyperplane and exited - /// before converting. The shapes are not in collision. - /// NoCollision: GJK converged and the shapes are not in collision. - /// Collision: GJK converged and the shapes are in collision. - /// Failed: GJK did not converge. - enum Status { - DidNotRun, - Failed, - NoCollisionEarlyStopped, - NoCollision, - CollisionWithPenetrationInformation, - Collision - }; - - public: - FCL_REAL distance_upper_bound; - Status status; - GJKVariant gjk_variant; - GJKConvergenceCriterion convergence_criterion; - GJKConvergenceCriterionType convergence_criterion_type; - - MinkowskiDiff const* shape; - Vec3f ray; - support_func_guess_t support_hint; - /// @brief The distance between the two shapes, computed by GJK. - /// If the distance is below GJK's threshold, the shapes are in collision in - /// the eyes of GJK. If `distance_upper_bound` is set to a value lower than - /// infinity, GJK will early stop as soon as it finds `distance` to be greater - /// than `distance_upper_bound`. - FCL_REAL distance; - Simplex* simplex; // Pointer to the result of the last run of GJK. - - private: - // max_iteration and tolerance are made private - // because they are meant to be set by the `reset` function. - size_t max_iterations; - FCL_REAL tolerance; - - SimplexV store_v[4]; - SimplexV* free_v[4]; - vertex_id_t nfree; - vertex_id_t current; - Simplex simplices[2]; - size_t iterations; - size_t iterations_momentum_stop; - - public: - /// \param max_iterations_ number of iteration before GJK returns failure. - /// \param tolerance_ precision of the algorithm. - /// - /// The tolerance argument is useful for continuous shapes and for polyhedron - /// with some vertices closer than this threshold. - /// - /// Suggested values are 100 iterations and a tolerance of 1e-6. - GJK(size_t max_iterations_, FCL_REAL tolerance_) - : max_iterations(max_iterations_), tolerance(tolerance_) { - HPP_FCL_ASSERT(tolerance_ > 0, "Tolerance must be positive.", - std::invalid_argument); - initialize(); - } - - /// @brief resets the GJK algorithm, preparing it for a new run. - /// Other than the maximum number of iterations and the tolerance, - /// this function does **not** modify the parameters of the GJK algorithm. - void reset(size_t max_iterations_, FCL_REAL tolerance_); - - /// @brief GJK algorithm, given the initial value guess - Status evaluate( - const MinkowskiDiff& shape, const Vec3f& guess, - const support_func_guess_t& supportHint = support_func_guess_t::Zero()); - - /// @brief apply the support function along a direction, the result is return - /// in sv - inline void getSupport(const Vec3f& d, SimplexV& sv, - support_func_guess_t& hint) const { - shape->support(d, sv.w0, sv.w1, hint); - sv.w = sv.w0 - sv.w1; - } - - /// @brief whether the simplex enclose the origin - bool encloseOrigin(); - - /// @brief get the underlying simplex using in GJK, can be used for cache in - /// next iteration - inline Simplex* getSimplex() const { return simplex; } - - /// Tells whether the closest points are available. - bool hasClosestPoints() const { return distance < distance_upper_bound; } - - /// Get the witness points on each object, and the corresponding normal. - /// @param[in] shape is the Minkowski difference of the two shapes. - /// @param[out] w0 is the witness point on shape0. - /// @param[out] w1 is the witness point on shape1. - /// @param[out] normal is the normal of the separating plane found by - /// GJK. It points from shape0 to shape1. - void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0, - Vec3f& w1, Vec3f& normal) const; - - /// @brief get the guess from current simplex - Vec3f getGuessFromSimplex() const; - - /// @brief Distance threshold for early break. - /// GJK stops when it proved the distance is more than this threshold. - /// @note The closest points will be erroneous in this case. - /// If you want the closest points, set this to infinity (the default). - void setDistanceEarlyBreak(const FCL_REAL& dup) { - distance_upper_bound = dup; - } - - /// @brief Convergence check used to stop GJK when shapes are not in - /// collision. - bool checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, - const FCL_REAL& omega) const; - - /// @brief Get the max number of iterations of GJK. - size_t getNumMaxIterations() const { return max_iterations; } - - /// @brief Get the tolerance of GJK. - FCL_REAL getTolerance() const { return tolerance; } - - /// @brief Get the number of iterations of the last run of GJK. - size_t getNumIterations() const { return iterations; } - - /// @brief Get GJK number of iterations before momentum stops. - /// Only usefull if the Nesterov or Polyak acceleration activated. - size_t getNumIterationsMomentumStopped() const { - return iterations_momentum_stop; - } - - private: - /// @brief Initializes the GJK algorithm. - /// This function should only be called by the constructor. - /// Otherwise use \ref reset. - void initialize(); - - /// @brief discard one vertex from the simplex - inline void removeVertex(Simplex& simplex); - - /// @brief append one vertex to the simplex - inline void appendVertex(Simplex& simplex, const Vec3f& v, - support_func_guess_t& hint); - - /// @brief Project origin (0) onto line a-b - /// For a detailed explanation of how to efficiently project onto a simplex, - /// check out Ericson's book, page 403: - /// https://realtimecollisiondetection.net/ To sum up, a simplex has a voronoi - /// region for each feature it has (vertex, edge, face). We find the voronoi - /// region in which the origin lies and stop as soon as we find it; we then - /// project onto it and return the result. We start by voronoi regions - /// generated by vertices then move on to edges then faces. Checking voronoi - /// regions is done using simple dot products. Moreover, edges voronoi checks - /// reuse computations of vertices voronoi checks. The same goes for faces - /// which reuse checks from edges. - /// Finally, in addition to the voronoi procedure, checks relying on the order - /// of construction - /// of the simplex are added. To know more about these, visit - /// https://caseymuratori.com/blog_0003. - bool projectLineOrigin(const Simplex& current, Simplex& next); - - /// @brief Project origin (0) onto triangle a-b-c - /// See \ref projectLineOrigin for an explanation on simplex projections. - bool projectTriangleOrigin(const Simplex& current, Simplex& next); - - /// @brief Project origin (0) onto tetrahedron a-b-c-d - /// See \ref projectLineOrigin for an explanation on simplex projections. - bool projectTetrahedraOrigin(const Simplex& current, Simplex& next); -}; - -/// @brief class for EPA algorithm -struct HPP_FCL_DLLAPI EPA { - typedef GJK::SimplexV SimplexVertex; - struct HPP_FCL_DLLAPI SimplexFace { - Vec3f n; - FCL_REAL d; - bool ignore; // If the origin does not project inside the face, we - // ignore this face. - size_t vertex_id[3]; // Index of vertex in sv_store. - SimplexFace* adjacent_faces[3]; // A face has three adjacent faces. - SimplexFace* prev_face; // The previous face in the list. - SimplexFace* next_face; // The next face in the list. - size_t adjacent_edge[3]; // Each face has 3 edges: `0`, `1` and `2`. - // The i-th adjacent face is bound (to this face) - // along its `adjacent_edge[i]`-th edge - // (with 0 <= i <= 2). - size_t pass; - - SimplexFace() : n(Vec3f::Zero()), ignore(false) {}; - }; - - /// @brief The simplex list of EPA is a linked list of faces. - /// Note: EPA's linked list does **not** own any memory. - /// The memory it refers to is contiguous and owned by a std::vector. - struct HPP_FCL_DLLAPI SimplexFaceList { - SimplexFace* root; - size_t count; - SimplexFaceList() : root(nullptr), count(0) {} - - void reset() { - root = nullptr; - count = 0; - } - - void append(SimplexFace* face) { - face->prev_face = nullptr; - face->next_face = root; - if (root != nullptr) root->prev_face = face; - root = face; - ++count; - } - - void remove(SimplexFace* face) { - if (face->next_face != nullptr) - face->next_face->prev_face = face->prev_face; - if (face->prev_face != nullptr) - face->prev_face->next_face = face->next_face; - if (face == root) root = face->next_face; - --count; - } - }; - - /// @brief We bind the face `fa` along its edge `ea` to the face `fb` along - /// its edge `fb`. - static inline void bind(SimplexFace* fa, size_t ea, SimplexFace* fb, - size_t eb) { - assert(ea == 0 || ea == 1 || ea == 2); - assert(eb == 0 || eb == 1 || eb == 2); - fa->adjacent_edge[ea] = eb; - fa->adjacent_faces[ea] = fb; - fb->adjacent_edge[eb] = ea; - fb->adjacent_faces[eb] = fa; - } - - struct HPP_FCL_DLLAPI SimplexHorizon { - SimplexFace* current_face; // current face in the horizon - SimplexFace* first_face; // first face in the horizon - size_t num_faces; // number of faces in the horizon - SimplexHorizon() - : current_face(nullptr), first_face(nullptr), num_faces(0) {} - }; - - enum Status { - DidNotRun = -1, - Failed = 0, - Valid = 1, - AccuracyReached = 1 << 1 | Valid, - Degenerated = 1 << 1 | Failed, - NonConvex = 2 << 1 | Failed, - InvalidHull = 3 << 1 | Failed, - OutOfFaces = 4 << 1 | Failed, - OutOfVertices = 5 << 1 | Failed, - FallBack = 6 << 1 | Failed - }; - - public: - Status status; - GJK::Simplex result; - Vec3f normal; - support_func_guess_t support_hint; - FCL_REAL depth; - SimplexFace* closest_face; - - private: - // max_iteration and tolerance are made private - // because they are meant to be set by the `reset` function. - size_t max_iterations; - FCL_REAL tolerance; - - std::vector sv_store; - std::vector fc_store; - SimplexFaceList hull, stock; - size_t num_vertices; // number of vertices in polytpoe constructed by EPA - size_t iterations; - - public: - EPA(size_t max_iterations_, FCL_REAL tolerance_) - : max_iterations(max_iterations_), tolerance(tolerance_) { - initialize(); - } - - /// @brief Copy constructor of EPA. - /// Mostly needed for the copy constructor of `GJKSolver`. - EPA(const EPA& other) - : max_iterations(other.max_iterations), - tolerance(other.tolerance), - sv_store(other.sv_store), - fc_store(other.fc_store) { - initialize(); - } - - /// @brief Get the max number of iterations of EPA. - size_t getNumMaxIterations() const { return max_iterations; } - - /// @brief Get the max number of vertices of EPA. - size_t getNumMaxVertices() const { return sv_store.size(); } - - /// @brief Get the max number of faces of EPA. - size_t getNumMaxFaces() const { return fc_store.size(); } - - /// @brief Get the tolerance of EPA. - FCL_REAL getTolerance() const { return tolerance; } - - /// @brief Get the number of iterations of the last run of EPA. - size_t getNumIterations() const { return iterations; } - - /// @brief Get the number of vertices in the polytope of the last run of EPA. - size_t getNumVertices() const { return num_vertices; } - - /// @brief Get the number of faces in the polytope of the last run of EPA. - size_t getNumFaces() const { return hull.count; } - - /// @brief resets the EPA algorithm, preparing it for a new run. - /// It potentially reallocates memory for the vertices and faces - /// if the passed parameters are bigger than the previous ones. - /// This function does **not** modify the parameters of the EPA algorithm, - /// i.e. the maximum number of iterations and the tolerance. - /// @note calling this function destroys the previous state of EPA. - /// In the future, we may want to copy it instead, i.e. when EPA will - /// be (properly) warm-startable. - void reset(size_t max_iterations, FCL_REAL tolerance); - - /// \return a Status which can be demangled using (status & Valid) or - /// (status & Failed). The other values provide a more detailled - /// status - Status evaluate(GJK& gjk, const Vec3f& guess); - - /// Get the witness points on each object, and the corresponding normal. - /// @param[in] shape is the Minkowski difference of the two shapes. - /// @param[out] w0 is the witness point on shape0. - /// @param[out] w1 is the witness point on shape1. - /// @param[in] normal is the normal found by EPA. It points from shape0 to - /// shape1. The normal is used to correct the witness points on the shapes if - /// the shapes have a non-zero swept-sphere radius. - void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0, - Vec3f& w1, Vec3f& normal) const; - - private: - /// @brief Allocates memory for the EPA algorithm. - /// This function should only be called by the constructor. - /// Otherwise use \ref reset. - void initialize(); - - bool getEdgeDist(SimplexFace* face, const SimplexVertex& a, - const SimplexVertex& b, FCL_REAL& dist); - - /// @brief Add a new face to the polytope. - /// This function sets the `ignore` flag to `true` if the origin does not - /// project inside the face. - SimplexFace* newFace(size_t id_a, size_t id_b, size_t id_vertex, - bool force = false); - - /// @brief Find the best polytope face to split - SimplexFace* findClosestFace(); - - /// @brief the goal is to add a face connecting vertex w and face edge f[e] - bool expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e, - SimplexHorizon& horizon); - - // @brief Use this function to debug expand if needed. - // void PrintExpandLooping(const SimplexFace* f, const SimplexVertex& w); -}; - -} // namespace details - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/narrowphase/minkowski_difference.h b/include/hpp/fcl/narrowphase/minkowski_difference.h index f6c0f93f4..5f6ca0e08 100644 --- a/include/hpp/fcl/narrowphase/minkowski_difference.h +++ b/include/hpp/fcl/narrowphase/minkowski_difference.h @@ -1,188 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * Copyright (c) 2021-2024, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ - -#ifndef HPP_FCL_MINKOWSKI_DIFFERENCE_H -#define HPP_FCL_MINKOWSKI_DIFFERENCE_H - -#include "hpp/fcl/shape/geometric_shapes.h" -#include "hpp/fcl/math/transform.h" -#include "hpp/fcl/narrowphase/support_functions.h" - -namespace hpp { -namespace fcl { - -namespace details { - -/// @brief Minkowski difference class of two shapes -/// -/// @note The Minkowski difference is expressed in the frame of the first shape. -struct HPP_FCL_DLLAPI MinkowskiDiff { - typedef Eigen::Array Array2d; - - /// @brief points to two shapes - const ShapeBase* shapes[2]; - - /// @brief Store temporary data for the computation of the support point for - /// each shape. - ShapeSupportData data[2]; - - /// @brief rotation from shape1 to shape0 - /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$. - Matrix3f oR1; - - /// @brief translation from shape1 to shape0 - /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$. - Vec3f ot1; - - /// @brief The radii of the sphere swepted around each shape of the Minkowski - /// difference. The 2 values correspond to the swept-sphere radius of shape 0 - /// and shape 1. - Array2d swept_sphere_radius; - - /// @brief Wether or not to use the normalize heuristic in the GJK Nesterov - /// acceleration. This setting is only applied if the Nesterov acceleration in - /// the GJK class is active. - bool normalize_support_direction; - - typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff, - const Vec3f& dir, Vec3f& support0, - Vec3f& support1, - support_func_guess_t& hint, - ShapeSupportData data[2]); - GetSupportFunction getSupportFunc; - - MinkowskiDiff() : normalize_support_direction(false), getSupportFunc(NULL) {} - - /// @brief Set the two shapes, assuming the relative transformation between - /// them is identity. - /// Consequently, all support points computed with the MinkowskiDiff - /// will be expressed in the world frame. - /// @param shape0 the first shape. - /// @param shape1 the second shape. - /// @tparam SupportOptions is a value of the SupportOptions enum. If set to - /// `WithSweptSphere`, the support computation will take into account the - /// swept sphere radius of the shapes. If set to `NoSweptSphere`, where - /// this information is simply stored in the Minkowski's difference - /// `swept_sphere_radius` array. This array is then used to correct the - /// solution found when GJK or EPA have converged. - /// - /// @note In practice, there is no need to take into account the swept-sphere - /// radius in the iterations of GJK/EPA. It is in fact detrimental to the - /// convergence of both algos. This is because it makes corners and edges of - /// shapes look strictly convex to the algorithms, which leads to poor - /// convergence. This swept sphere template parameter is only here for - /// debugging purposes and for specific uses cases where the swept sphere - /// radius is needed in the support function. The rule is simple. When - /// interacting with GJK/EPA, the `SupportOptions` template - /// parameter should **always** be set to `NoSweptSphere` (except for - /// debugging or testing purposes). When working directly with the shapes - /// involved in the collision, and not relying on GJK/EPA, the - /// `SupportOptions` template parameter should be set to `WithSweptSphere`. - /// This is for example the case for specialized collision/distance functions. - template - void set(const ShapeBase* shape0, const ShapeBase* shape1); - - /// @brief Set the two shapes, with a relative transformation from shape0 to - /// shape1. Consequently, all support points computed with the MinkowskiDiff - /// will be expressed in the frame of shape0. - /// @param shape0 the first shape. - /// @param shape1 the second shape. - /// @param tf0 the transformation of the first shape. - /// @param tf1 the transformation of the second shape. - /// @tparam `SupportOptions` see `set(const ShapeBase*, const - /// ShapeBase*)` for more details. - template - void set(const ShapeBase* shape0, const ShapeBase* shape1, - const Transform3f& tf0, const Transform3f& tf1); - - /// @brief support function for shape0. - /// The output vector is expressed in the local frame of shape0. - /// @return a point which belongs to the set {argmax_{v in shape0} - /// v.dot(dir)}, i.e a support of shape0 in direction dir. - /// @param dir support direction. - /// @param hint used to initialize the search when shape is a ConvexBase - /// object. - /// @tparam `SupportOptions` see `set(const ShapeBase*, const - /// ShapeBase*)` for more details. - template - inline Vec3f support0(const Vec3f& dir, int& hint) const { - return getSupport<_SupportOptions>(shapes[0], dir, hint); - } - - /// @brief support function for shape1. - /// The output vector is expressed in the local frame of shape0. - /// This is mandatory because in the end we are interested in support points - /// of the Minkowski difference; hence the supports of shape0 and shape1 must - /// live in the same frame. - /// @return a point which belongs to the set {tf * argmax_{v in shape1} - /// v.dot(R^T * dir)}, i.e. the support of shape1 in direction dir (tf is the - /// tranform from shape1 to shape0). - /// @param dir support direction. - /// @param hint used to initialize the search when shape is a ConvexBase. - /// @tparam `SupportOptions` see `set(const ShapeBase*, const - /// ShapeBase*)` for more details. - template - inline Vec3f support1(const Vec3f& dir, int& hint) const { - // clang-format off - return oR1 * getSupport<_SupportOptions>(shapes[1], oR1.transpose() * dir, hint) + ot1; - // clang-format on - } - - /// @brief Support function for the pair of shapes. This method assumes `set` - /// has already been called. - /// @param[in] dir the support direction. - /// @param[out] supp0 support of shape0 in direction dir, expressed in the - /// frame of shape0. - /// @param[out] supp1 support of shape1 in direction -dir, expressed in the - /// frame of shape0. - /// @param[in/out] hint used to initialize the search when shape is a - /// ConvexBase object. - inline void support(const Vec3f& dir, Vec3f& supp0, Vec3f& supp1, - support_func_guess_t& hint) const { - assert(getSupportFunc != NULL); - getSupportFunc(*this, dir, supp0, supp1, hint, - const_cast(data)); - } -}; - -} // namespace details - -} // namespace fcl -} // namespace hpp - -#endif // HPP_FCL_MINKOWSKI_DIFFERENCE_H +#include +#include diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index ff9a5785b..0a079085d 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -1,729 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * Copyright (c) 2018-2019, Centre National de la Recherche Scientifique - * All rights reserved. - * Copyright (c) 2021-2024, INRIA - * All rights reserved. - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan, Florent Lamiraux */ - -#ifndef HPP_FCL_NARROWPHASE_H -#define HPP_FCL_NARROWPHASE_H - -#include - -#include -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// @brief collision and distance solver based on the GJK and EPA algorithms. -/// Originally, GJK and EPA were implemented in fcl which itself took -/// inspiration from the code of the GJK in bullet. Since then, both GJK and EPA -/// have been largely modified to be faster and more robust to numerical -/// accuracy and edge cases. -struct HPP_FCL_DLLAPI GJKSolver { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /// @brief GJK algorithm - mutable details::GJK gjk; - - /// @brief maximum number of iterations of GJK - size_t gjk_max_iterations; - - /// @brief tolerance of GJK - FCL_REAL gjk_tolerance; - - /// @brief which warm start to use for GJK - GJKInitialGuess gjk_initial_guess; - - /// @brief Whether smart guess can be provided - /// @Deprecated Use gjk_initial_guess instead - HPP_FCL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead) - bool enable_cached_guess; - - /// @brief smart guess - mutable Vec3f cached_guess; - - /// @brief smart guess for the support function - mutable support_func_guess_t support_func_cached_guess; - - /// @brief If GJK can guarantee that the distance between the shapes is - /// greater than this value, it will early stop. - FCL_REAL distance_upper_bound; - - /// @brief Variant of the GJK algorithm (Default, Nesterov or Polyak). - GJKVariant gjk_variant; - - /// @brief Convergence criterion for GJK - GJKConvergenceCriterion gjk_convergence_criterion; - - /// @brief Absolute or relative convergence criterion for GJK - GJKConvergenceCriterionType gjk_convergence_criterion_type; - - /// @brief EPA algorithm - mutable details::EPA epa; - - /// @brief maximum number of iterations of EPA - size_t epa_max_iterations; - - /// @brief tolerance of EPA - FCL_REAL epa_tolerance; - - /// @brief Minkowski difference used by GJK and EPA algorithms - mutable details::MinkowskiDiff minkowski_difference; - - private: - // Used internally for assertion checks. - static constexpr FCL_REAL m_dummy_precision = 1e-6; - - public: - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - /// @brief Default constructor for GJK algorithm - /// By default, we don't want EPA to allocate memory because - /// certain functions of the `GJKSolver` class have specializations - /// which don't use EPA (and/or GJK). - /// So we give EPA's constructor a max number of iterations of zero. - /// Only the functions that need EPA will reset the algorithm and allocate - /// memory if needed. - GJKSolver() - : gjk(GJK_DEFAULT_MAX_ITERATIONS, GJK_DEFAULT_TOLERANCE), - gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS), - gjk_tolerance(GJK_DEFAULT_TOLERANCE), - gjk_initial_guess(GJKInitialGuess::DefaultGuess), - enable_cached_guess(false), // Use gjk_initial_guess instead - cached_guess(Vec3f(1, 0, 0)), - support_func_cached_guess(support_func_guess_t::Zero()), - distance_upper_bound((std::numeric_limits::max)()), - gjk_variant(GJKVariant::DefaultGJK), - gjk_convergence_criterion(GJKConvergenceCriterion::Default), - gjk_convergence_criterion_type(GJKConvergenceCriterionType::Absolute), - epa(0, EPA_DEFAULT_TOLERANCE), - epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS), - epa_tolerance(EPA_DEFAULT_TOLERANCE) {} - - /// @brief Constructor from a DistanceRequest - /// - /// \param[in] request DistanceRequest input - /// - /// See the default constructor; by default, we don't want - /// EPA to allocate memory so we call EPA's constructor with 0 max - /// number of iterations. - /// However, the `set` method stores the actual values of the request. - /// EPA will thus allocate memory only if needed. - explicit GJKSolver(const DistanceRequest& request) - : gjk(request.gjk_max_iterations, request.gjk_tolerance), - epa(0, request.epa_tolerance) { - this->cached_guess = Vec3f(1, 0, 0); - this->support_func_cached_guess = support_func_guess_t::Zero(); - - set(request); - } - - /// @brief setter from a DistanceRequest - /// - /// \param[in] request DistanceRequest input - /// - void set(const DistanceRequest& request) { - // --------------------- - // GJK settings - this->gjk_initial_guess = request.gjk_initial_guess; - this->enable_cached_guess = request.enable_cached_gjk_guess; - if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess || - this->enable_cached_guess) { - this->cached_guess = request.cached_gjk_guess; - this->support_func_cached_guess = request.cached_support_func_guess; - } - this->gjk_max_iterations = request.gjk_max_iterations; - this->gjk_tolerance = request.gjk_tolerance; - // For distance computation, we don't want GJK to early stop - this->distance_upper_bound = (std::numeric_limits::max)(); - this->gjk_variant = request.gjk_variant; - this->gjk_convergence_criterion = request.gjk_convergence_criterion; - this->gjk_convergence_criterion_type = - request.gjk_convergence_criterion_type; - - // --------------------- - // EPA settings - this->epa_max_iterations = request.epa_max_iterations; - this->epa_tolerance = request.epa_tolerance; - - // --------------------- - // Reset GJK and EPA status - this->epa.status = details::EPA::Status::DidNotRun; - this->gjk.status = details::GJK::Status::DidNotRun; - } - - /// @brief Constructor from a CollisionRequest - /// - /// \param[in] request CollisionRequest input - /// - /// See the default constructor; by default, we don't want - /// EPA to allocate memory so we call EPA's constructor with 0 max - /// number of iterations. - /// However, the `set` method stores the actual values of the request. - /// EPA will thus allocate memory only if needed. - explicit GJKSolver(const CollisionRequest& request) - : gjk(request.gjk_max_iterations, request.gjk_tolerance), - epa(0, request.epa_tolerance) { - this->cached_guess = Vec3f(1, 0, 0); - this->support_func_cached_guess = support_func_guess_t::Zero(); - - set(request); - } - - /// @brief setter from a CollisionRequest - /// - /// \param[in] request CollisionRequest input - /// - void set(const CollisionRequest& request) { - // --------------------- - // GJK settings - this->gjk_initial_guess = request.gjk_initial_guess; - this->enable_cached_guess = request.enable_cached_gjk_guess; - if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess || - this->enable_cached_guess) { - this->cached_guess = request.cached_gjk_guess; - this->support_func_cached_guess = request.cached_support_func_guess; - } - this->gjk_tolerance = request.gjk_tolerance; - this->gjk_max_iterations = request.gjk_max_iterations; - // The distance upper bound should be at least greater to the requested - // security margin. Otherwise, we will likely miss some collisions. - this->distance_upper_bound = (std::max)( - 0., (std::max)(request.distance_upper_bound, request.security_margin)); - this->gjk_variant = request.gjk_variant; - this->gjk_convergence_criterion = request.gjk_convergence_criterion; - this->gjk_convergence_criterion_type = - request.gjk_convergence_criterion_type; - - // --------------------- - // EPA settings - this->epa_max_iterations = request.epa_max_iterations; - this->epa_tolerance = request.epa_tolerance; - - // --------------------- - // Reset GJK and EPA status - this->gjk.status = details::GJK::Status::DidNotRun; - this->epa.status = details::EPA::Status::DidNotRun; - } - - /// @brief Copy constructor - GJKSolver(const GJKSolver& other) = default; - - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - bool operator==(const GJKSolver& other) const { - return this->enable_cached_guess == - other.enable_cached_guess && // use gjk_initial_guess instead - this->cached_guess == other.cached_guess && - this->support_func_cached_guess == other.support_func_cached_guess && - this->gjk_max_iterations == other.gjk_max_iterations && - this->gjk_tolerance == other.gjk_tolerance && - this->distance_upper_bound == other.distance_upper_bound && - this->gjk_variant == other.gjk_variant && - this->gjk_convergence_criterion == other.gjk_convergence_criterion && - this->gjk_convergence_criterion_type == - other.gjk_convergence_criterion_type && - this->gjk_initial_guess == other.gjk_initial_guess && - this->epa_max_iterations == other.epa_max_iterations && - this->epa_tolerance == other.epa_tolerance; - } - HPP_FCL_COMPILER_DIAGNOSTIC_POP - - bool operator!=(const GJKSolver& other) const { return !(*this == other); } - - /// @brief Helper to return the precision of the solver on the distance - /// estimate, depending on whether or not `compute_penetration` is true. - FCL_REAL getDistancePrecision(const bool compute_penetration) const { - return compute_penetration - ? (std::max)(this->gjk_tolerance, this->epa_tolerance) - : this->gjk_tolerance; - } - - /// @brief Uses GJK and EPA to compute the distance between two shapes. - /// @param `s1` the first shape. - /// @param `tf1` the transformation of the first shape. - /// @param `s2` the second shape. - /// @param `tf2` the transformation of the second shape. - /// @param `compute_penetration` if true and GJK finds the shape in collision, - /// the EPA algorithm is also ran to compute penetration information. - /// @param[out] `p1` the witness point on the first shape. - /// @param[out] `p2` the witness point on the second shape. - /// @param[out] `normal` the normal of the collision, pointing from the first - /// to the second shape. - /// @return the estimate of the distance between the two shapes. - /// - /// @note: if `this->distance_upper_bound` is set to a positive value, GJK - /// will early stop if it finds the distance to be above this value. The - /// distance returned by `this->shapeDistance` will be a lower bound on the - /// distance between the two shapes. - /// - /// @note: the variables `this->gjk.status` and `this->epa.status` can be used - /// to examine the status of GJK and EPA. - /// - /// @note: GJK and EPA give an estimate of the distance between the two - /// shapes. This estimate is precise up to the tolerance of the algorithms: - /// - If `compute_penetration` is false, the distance is precise up to - /// `gjk_tolerance`. - /// - If `compute_penetration` is true, the distance is precise up to - /// `std::max(gjk_tolerance, epa_tolerance)` - /// It's up to the user to decide whether the shapes are in collision or not, - /// based on that estimate. - template - FCL_REAL shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, const bool compute_penetration, - Vec3f& p1, Vec3f& p2, Vec3f& normal) const { - constexpr bool relative_transformation_already_computed = false; - FCL_REAL distance; - this->runGJKAndEPA(s1, tf1, s2, tf2, compute_penetration, distance, p1, p2, - normal, relative_transformation_already_computed); - return distance; - } - - /// @brief Partial specialization of `shapeDistance` for the case where the - /// second shape is a triangle. It is more efficient to pre-compute the - /// relative transformation between the two shapes before calling GJK/EPA. - template - FCL_REAL shapeDistance(const S1& s1, const Transform3f& tf1, - const TriangleP& s2, const Transform3f& tf2, - const bool compute_penetration, Vec3f& p1, Vec3f& p2, - Vec3f& normal) const { - const Transform3f tf_1M2(tf1.inverseTimes(tf2)); - TriangleP tri(tf_1M2.transform(s2.a), tf_1M2.transform(s2.b), - tf_1M2.transform(s2.c)); - - constexpr bool relative_transformation_already_computed = true; - FCL_REAL distance; - this->runGJKAndEPA(s1, tf1, tri, tf_1M2, compute_penetration, distance, p1, - p2, normal, relative_transformation_already_computed); - return distance; - } - - /// @brief See other partial template specialization of shapeDistance above. - template - FCL_REAL shapeDistance(const TriangleP& s1, const Transform3f& tf1, - const S2& s2, const Transform3f& tf2, - const bool compute_penetration, Vec3f& p1, Vec3f& p2, - Vec3f& normal) const { - FCL_REAL distance = this->shapeDistance( - s2, tf2, s1, tf1, compute_penetration, p2, p1, normal); - normal = -normal; - return distance; - } - - protected: - /// @brief initialize GJK. - /// This method assumes `minkowski_difference` has been set. - template - void getGJKInitialGuess(const S1& s1, const S2& s2, Vec3f& guess, - support_func_guess_t& support_hint, - const Vec3f& default_guess = Vec3f(1, 0, 0)) const { - // There is no reason not to warm-start the support function, so we always - // do it. - support_hint = this->support_func_cached_guess; - // The following switch takes care of the GJK warm-start. - switch (gjk_initial_guess) { - case GJKInitialGuess::DefaultGuess: - guess = default_guess; - break; - case GJKInitialGuess::CachedGuess: - guess = this->cached_guess; - break; - case GJKInitialGuess::BoundingVolumeGuess: - if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) { - HPP_FCL_THROW_PRETTY( - "computeLocalAABB must have been called on the shapes before " - "using " - "GJKInitialGuess::BoundingVolumeGuess.", - std::logic_error); - } - guess.noalias() = - s1.aabb_local.center() - - (this->minkowski_difference.oR1 * s2.aabb_local.center() + - this->minkowski_difference.ot1); - break; - default: - HPP_FCL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error); - } - // TODO: use gjk_initial_guess instead - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - if (this->enable_cached_guess) { - guess = this->cached_guess; - } - HPP_FCL_COMPILER_DIAGNOSTIC_POP - } - - /// @brief Runs the GJK algorithm. - /// @param `s1` the first shape. - /// @param `tf1` the transformation of the first shape. - /// @param `s2` the second shape. - /// @param `tf2` the transformation of the second shape. - /// @param `compute_penetration` if true and if the shapes are in found in - /// collision, the EPA algorithm is also ran to compute penetration - /// information. - /// @param[out] `distance` the distance between the two shapes. - /// @param[out] `p1` the witness point on the first shape. - /// @param[out] `p2` the witness point on the second shape. - /// @param[out] `normal` the normal of the collision, pointing from the first - /// to the second shape. - /// @param `relative_transformation_already_computed` whether the relative - /// transformation between the two shapes has already been computed. - /// @tparam SupportOptions, see `MinkowskiDiff::set`. Whether the support - /// computations should take into account the shapes' swept-sphere radii - /// during the iterations of GJK and EPA. Please leave this default value to - /// `false` unless you know what you are doing. This template parameter is - /// only used for debugging/testing purposes. In short, there is no need to - /// take into account the swept sphere radius when computing supports in the - /// iterations of GJK and EPA. GJK and EPA will correct the solution once they - /// have converged. - /// @return the estimate of the distance between the two shapes. - /// - /// @note: The variables `this->gjk.status` and `this->epa.status` can be used - /// to examine the status of GJK and EPA. - template - void runGJKAndEPA( - const S1& s1, const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, const bool compute_penetration, - FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal, - const bool relative_transformation_already_computed = false) const { - // Reset internal state of GJK algorithm - if (relative_transformation_already_computed) - this->minkowski_difference.set<_SupportOptions>(&s1, &s2); - else - this->minkowski_difference.set<_SupportOptions>(&s1, &s2, tf1, tf2); - this->gjk.reset(this->gjk_max_iterations, this->gjk_tolerance); - this->gjk.setDistanceEarlyBreak(this->distance_upper_bound); - this->gjk.gjk_variant = this->gjk_variant; - this->gjk.convergence_criterion = this->gjk_convergence_criterion; - this->gjk.convergence_criterion_type = this->gjk_convergence_criterion_type; - this->epa.status = details::EPA::Status::DidNotRun; - - // Get initial guess for GJK: default, cached or bounding volume guess - Vec3f guess; - support_func_guess_t support_hint; - getGJKInitialGuess(*(this->minkowski_difference.shapes[0]), - *(this->minkowski_difference.shapes[1]), guess, - support_hint); - - this->gjk.evaluate(this->minkowski_difference, guess, support_hint); - - switch (this->gjk.status) { - case details::GJK::DidNotRun: - HPP_FCL_ASSERT(false, "GJK did not run. It should have!", - std::logic_error); - this->cached_guess = Vec3f(1, 0, 0); - this->support_func_cached_guess.setZero(); - distance = -(std::numeric_limits::max)(); - p1 = p2 = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); - break; - case details::GJK::Failed: - // - // GJK ran out of iterations. - HPP_FCL_LOG_WARNING("GJK ran out of iterations."); - GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); - break; - case details::GJK::NoCollisionEarlyStopped: - // - // Case where GJK early stopped because the distance was found to be - // above the `distance_upper_bound`. - // The two witness points have no meaning. - GJKEarlyStopExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - HPP_FCL_ASSERT(distance >= this->gjk.distance_upper_bound - - this->m_dummy_precision, - "The distance should be bigger than GJK's " - "`distance_upper_bound`.", - std::logic_error); - break; - case details::GJK::NoCollision: - // - // Case where GJK converged and proved that the shapes are not in - // collision, i.e their distance is above GJK's tolerance (default - // 1e-6). - GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); - HPP_FCL_ASSERT(std::abs((p1 - p2).norm() - distance) <= - this->gjk.getTolerance() + this->m_dummy_precision, - "The distance found by GJK should coincide with the " - "distance between the closest points.", - std::logic_error); - break; - // - // Next are the cases where GJK found the shapes to be in collision, i.e. - // their distance is below GJK's tolerance (default 1e-6). - case details::GJK::CollisionWithPenetrationInformation: - GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); - HPP_FCL_ASSERT( - distance <= this->gjk.getTolerance() + this->m_dummy_precision, - "The distance found by GJK should be negative or at " - "least below GJK's tolerance.", - std::logic_error); - break; - case details::GJK::Collision: - if (!compute_penetration) { - // Skip EPA and set the witness points and the normal to nans. - GJKCollisionExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - } else { - // - // GJK was not enough to recover the penetration information. - // We need to run the EPA algorithm to find the witness points, - // penetration depth and the normal. - - // Reset EPA algorithm. Potentially allocate memory if - // `epa_max_face_num` or `epa_max_vertex_num` are bigger than EPA's - // current storage. - this->epa.reset(this->epa_max_iterations, this->epa_tolerance); - - // TODO: understand why EPA's performance is so bad on cylinders and - // cones. - this->epa.evaluate(this->gjk, -guess); - - switch (epa.status) { - // - // In the following switch cases, until the "Valid" case, - // EPA either ran out of iterations, of faces or of vertices. - // The depth, witness points and the normal are still valid, - // simply not at the precision of EPA's tolerance. - // The flag `HPP_FCL_ENABLE_LOGGING` enables feebdack on these - // cases. - // - // TODO: Remove OutOfFaces and OutOfVertices statuses and simply - // compute the upper bound on max faces and max vertices as a - // function of the number of iterations. - case details::EPA::OutOfFaces: - HPP_FCL_LOG_WARNING("EPA ran out of faces."); - EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); - break; - case details::EPA::OutOfVertices: - HPP_FCL_LOG_WARNING("EPA ran out of vertices."); - EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); - break; - case details::EPA::Failed: - HPP_FCL_LOG_WARNING("EPA ran out of iterations."); - EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); - break; - case details::EPA::Valid: - case details::EPA::AccuracyReached: - HPP_FCL_ASSERT( - -epa.depth <= epa.getTolerance() + this->m_dummy_precision, - "EPA's penetration distance should be negative (or " - "at least below EPA's tolerance).", - std::logic_error); - EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); - break; - case details::EPA::Degenerated: - HPP_FCL_LOG_WARNING( - "EPA warning: created a polytope with a degenerated face."); - EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); - break; - case details::EPA::NonConvex: - HPP_FCL_LOG_WARNING( - "EPA warning: EPA got called onto non-convex shapes."); - EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); - break; - case details::EPA::InvalidHull: - HPP_FCL_LOG_WARNING("EPA warning: created an invalid polytope."); - EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); - break; - case details::EPA::DidNotRun: - HPP_FCL_ASSERT(false, "EPA did not run. It should have!", - std::logic_error); - HPP_FCL_LOG_ERROR("EPA error: did not run. It should have."); - EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - break; - case details::EPA::FallBack: - HPP_FCL_ASSERT( - false, - "EPA went into fallback mode. It should never do that.", - std::logic_error); - HPP_FCL_LOG_ERROR("EPA error: FallBack."); - EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - break; - } - } - break; // End of case details::GJK::Collision - } - } - - void GJKEarlyStopExtractWitnessPointsAndNormal(const Transform3f& tf1, - FCL_REAL& distance, Vec3f& p1, - Vec3f& p2, - Vec3f& normal) const { - HPP_FCL_UNUSED_VARIABLE(tf1); - // Cache gjk result for potential future call to this GJKSolver. - this->cached_guess = this->gjk.ray; - this->support_func_cached_guess = this->gjk.support_hint; - - distance = this->gjk.distance; - p1 = p2 = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); - // If we absolutely want to return some witness points, we could use - // the following code (or simply merge the early stopped case with the - // valid case below): - // gjk.getWitnessPointsAndNormal(minkowski_difference, p1, p2, normal); - // p1 = tf1.transform(p1); - // p2 = tf1.transform(p2); - // normal = tf1.getRotation() * normal; - } - - void GJKExtractWitnessPointsAndNormal(const Transform3f& tf1, - FCL_REAL& distance, Vec3f& p1, - Vec3f& p2, Vec3f& normal) const { - // Apart from early stopping, there are two cases where GJK says there is no - // collision: - // 1. GJK proved the distance is above its tolerance (default 1e-6). - // 2. GJK ran out of iterations. - // In any case, `gjk.ray`'s norm is bigger than GJK's tolerance and thus - // it can safely be normalized. - HPP_FCL_ASSERT( - this->gjk.ray.norm() > - this->gjk.getTolerance() - this->m_dummy_precision, - "The norm of GJK's ray should be bigger than GJK's tolerance.", - std::logic_error); - // Cache gjk result for potential future call to this GJKSolver. - this->cached_guess = this->gjk.ray; - this->support_func_cached_guess = this->gjk.support_hint; - - distance = this->gjk.distance; - // TODO: On degenerated case, the closest points may be non-unique. - // (i.e. an object face normal is colinear to `gjk.ray`) - gjk.getWitnessPointsAndNormal(this->minkowski_difference, p1, p2, normal); - Vec3f p = tf1.transform(0.5 * (p1 + p2)); - normal = tf1.getRotation() * normal; - p1.noalias() = p - 0.5 * distance * normal; - p2.noalias() = p + 0.5 * distance * normal; - } - - void GJKCollisionExtractWitnessPointsAndNormal(const Transform3f& tf1, - FCL_REAL& distance, Vec3f& p1, - Vec3f& p2, - Vec3f& normal) const { - HPP_FCL_UNUSED_VARIABLE(tf1); - HPP_FCL_ASSERT(this->gjk.distance <= - this->gjk.getTolerance() + this->m_dummy_precision, - "The distance should be lower than GJK's tolerance.", - std::logic_error); - // Because GJK has returned the `Collision` status and EPA has not run, - // we purposefully do not cache the result of GJK, because ray is zero. - // However, we can cache the support function hint. - // this->cached_guess = this->gjk.ray; - this->support_func_cached_guess = this->gjk.support_hint; - - distance = this->gjk.distance; - p1 = p2 = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); - } - - void EPAExtractWitnessPointsAndNormal(const Transform3f& tf1, - FCL_REAL& distance, Vec3f& p1, - Vec3f& p2, Vec3f& normal) const { - // Cache EPA result for potential future call to this GJKSolver. - // This caching allows to warm-start the next GJK call. - this->cached_guess = -(this->epa.depth * this->epa.normal); - this->support_func_cached_guess = this->epa.support_hint; - distance = (std::min)(0., -this->epa.depth); - this->epa.getWitnessPointsAndNormal(this->minkowski_difference, p1, p2, - normal); - // The following is very important to understand why EPA can sometimes - // return a normal that is not colinear to the vector $p_1 - p_2$ when - // working with tolerances like $\epsilon = 10^{-3}$. - // It can be resumed with a simple idea: - // EPA is an algorithm meant to find the penetration depth and the - // normal. It is not meant to find the closest points. - // Again, the issue here is **not** the normal, it's $p_1$ and $p_2$. - // - // More details: - // We'll denote $S_1$ and $S_2$ the two shapes, $n$ the normal and $p_1$ and - // $p_2$ the witness points. In theory, when EPA converges to $\epsilon = - // 0$, the normal and witness points verify the following property (P): - // - $p_1 \in \partial \sigma_{S_1}(n)$, - // - $p_2 \in \partial \sigma_{S_2}(-n), - // where $\sigma_{S_1}$ and $\sigma_{S_2}$ are the support functions of - // $S_1$ and $S_2$. The $\partial \sigma(n)$ simply denotes the support set - // of the support function in the direction $n$. (Note: I am leaving out the - // details of frame choice for the support function, to avoid making the - // mathematical notation too heavy.) - // --> In practice, EPA converges to $\epsilon > 0$. - // On polytopes and the likes, this does not change much and the property - // given above is still valid. - // --> However, this is very different on curved surfaces, such as - // ellipsoids, cylinders, cones, capsules etc. For these shapes, converging - // at $\epsilon = 10^{-6}$ or to $\epsilon = 10^{-3}$ does not change the - // normal much, but the property (P) given above is no longer valid, which - // means that the points $p_1$ and $p_2$ do not necessarily belong to the - // support sets in the direction of $n$ and thus $n$ and $p_1 - p_2$ are not - // colinear. - // - // Do not panic! This is fine. - // Although the property above is not verified, it's almost verified, - // meaning that $p_1$ and $p_2$ belong to support sets in directions that - // are very close to $n$. - // - // Solution to compute better $p_1$ and $p_2$: - // We compute the middle points of the current $p_1$ and $p_2$ and we use - // the normal and the distance given by EPA to compute the new $p_1$ and - // $p_2$. - Vec3f p = tf1.transform(0.5 * (p1 + p2)); - normal = tf1.getRotation() * normal; - p1.noalias() = p - 0.5 * distance * normal; - p2.noalias() = p + 0.5 * distance * normal; - } - - void EPAFailedExtractWitnessPointsAndNormal(const Transform3f& tf1, - FCL_REAL& distance, Vec3f& p1, - Vec3f& p2, Vec3f& normal) const { - this->cached_guess = Vec3f(1, 0, 0); - this->support_func_cached_guess.setZero(); - - HPP_FCL_UNUSED_VARIABLE(tf1); - distance = -(std::numeric_limits::max)(); - p1 = p2 = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); - } -}; - -} // namespace fcl -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/narrowphase/narrowphase_defaults.h b/include/hpp/fcl/narrowphase/narrowphase_defaults.h index fcf3cd41c..c17096f9a 100644 --- a/include/hpp/fcl/narrowphase/narrowphase_defaults.h +++ b/include/hpp/fcl/narrowphase/narrowphase_defaults.h @@ -1,67 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2024, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/// This file defines different macros used to characterize the default behavior -/// of the narrowphase algorithms GJK and EPA. - -#ifndef HPP_FCL_NARROWPHASE_DEFAULTS -#define HPP_FCL_NARROWPHASE_DEFAULTS - -#include - -namespace hpp { -namespace fcl { - -/// GJK -constexpr size_t GJK_DEFAULT_MAX_ITERATIONS = 128; -constexpr FCL_REAL GJK_DEFAULT_TOLERANCE = 1e-6; -/// Note: if the considered shapes are on the order of the meter, and the -/// convergence criterion of GJK is the default VDB criterion, -/// setting a tolerance of 1e-6 on the GJK algorithm makes it precise up to -/// the micro-meter. -/// The same is true for EPA. -constexpr FCL_REAL GJK_MINIMUM_TOLERANCE = 1e-6; - -/// EPA -/// EPA build a polytope which maximum size is: -/// - `#iterations + 4` vertices -/// - `2 x #iterations + 4` faces -constexpr size_t EPA_DEFAULT_MAX_ITERATIONS = 64; -constexpr FCL_REAL EPA_DEFAULT_TOLERANCE = 1e-6; -constexpr FCL_REAL EPA_MINIMUM_TOLERANCE = 1e-6; - -} // namespace fcl -} // namespace hpp - -#endif // HPP_FCL_NARROWPHASE_DEFAULTS +#include +#include diff --git a/include/hpp/fcl/narrowphase/support_functions.h b/include/hpp/fcl/narrowphase/support_functions.h index 951240cf5..8eb7c4b4f 100644 --- a/include/hpp/fcl/narrowphase/support_functions.h +++ b/include/hpp/fcl/narrowphase/support_functions.h @@ -1,310 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * Copyright (c) 2021-2024, INRIA - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ - -#ifndef HPP_FCL_SUPPORT_FUNCTIONS_H -#define HPP_FCL_SUPPORT_FUNCTIONS_H - -#include "hpp/fcl/shape/geometric_shapes.h" -#include "hpp/fcl/math/transform.h" -#include "hpp/fcl/collision_data.h" - -namespace hpp { -namespace fcl { - -namespace details { - -/// @brief Options for the computation of support points. -/// `NoSweptSphere` option is used when the support function is called -/// by GJK or EPA. In this case, the swept sphere radius is not taken into -/// account in the support function. It is used by GJK and EPA after they have -/// converged to correct the solution. -/// `WithSweptSphere` option is used when the support function is called -/// directly by the user. In this case, the swept sphere radius is taken into -/// account in the support function. -enum SupportOptions { - NoSweptSphere = 0, - WithSweptSphere = 0x1, -}; - -// ============================================================================ -// ============================ SUPPORT FUNCTIONS ============================= -// ============================================================================ -/// @brief the support function for shape. -/// The output support point is expressed in the local frame of the shape. -/// @return a point which belongs to the set {argmax_{v in shape} v.dot(dir)}. -/// @param shape the shape. -/// @param dir support direction. -/// @param hint used to initialize the search when shape is a ConvexBase object. -/// @tparam SupportOptions is a value of the SupportOptions enum. If set to -/// `WithSweptSphere`, the support functions take into account the shapes' swept -/// sphere radii. Please see `MinkowskiDiff::set(const ShapeBase*, const -/// ShapeBase*)` for more details. -template -Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint); - -/// @brief Stores temporary data for the computation of support points. -struct HPP_FCL_DLLAPI ShapeSupportData { - // @brief Tracks which points have been visited in a ConvexBase. - std::vector visited; - - // @brief Tracks the last support direction used on this shape; used to - // warm-start the ConvexBase support function. - Vec3f last_dir = Vec3f::Zero(); - - // @brief Temporary set used to compute the convex-hull of a support set. - // Only used for ConvexBase and Box. - SupportSet::Polygon polygon; -}; - -/// @brief Triangle support function. -template -void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, - Vec3f& support, int& /*unused*/, - ShapeSupportData& /*unused*/); - -/// @brief Box support function. -template -void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, - int& /*unused*/, ShapeSupportData& /*unused*/); - -/// @brief Sphere support function. -template -void getShapeSupport(const Sphere* sphere, const Vec3f& dir, Vec3f& support, - int& /*unused*/, ShapeSupportData& /*unused*/); - -/// @brief Ellipsoid support function. -template -void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir, - Vec3f& support, int& /*unused*/, - ShapeSupportData& /*unused*/); - -/// @brief Capsule support function. -template -void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support, - int& /*unused*/, ShapeSupportData& /*unused*/); - -/// @brief Cone support function. -template -void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, - int& /*unused*/, ShapeSupportData& /*unused*/); - -/// @brief Cylinder support function. -template -void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, - int& /*unused*/, ShapeSupportData& /*unused*/); - -/// @brief ConvexBase support function. -/// @note See @ref LargeConvex and SmallConvex to see how to optimize -/// ConvexBase's support computation. -template -void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, - int& hint, ShapeSupportData& /*unused*/); - -/// @brief Cast a `ConvexBase` to a `LargeConvex` to use the log version of -/// `getShapeSupport`. This is **much** faster than the linear version of -/// `getShapeSupport` when a `ConvexBase` has more than a few dozen of vertices. -/// @note WARNING: when using a LargeConvex, the neighbors in `ConvexBase` must -/// have been constructed! Otherwise the support function will segfault. -struct LargeConvex : ShapeBase {}; -/// @brief See @ref LargeConvex. -struct SmallConvex : ShapeBase {}; - -/// @brief Support function for large ConvexBase (>32 vertices). -template -void getShapeSupport(const SmallConvex* convex, const Vec3f& dir, - Vec3f& support, int& hint, ShapeSupportData& data); - -/// @brief Support function for small ConvexBase (<32 vertices). -template -void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, - Vec3f& support, int& hint, ShapeSupportData& support_data); - -// ============================================================================ -// ========================== SUPPORT SET FUNCTIONS =========================== -// ============================================================================ -/// @brief Computes the support set for shape. -/// This function assumes the frame of the support set has already been -/// computed and that this frame is expressed w.r.t the local frame of the -/// shape (i.e. the local frame of the shape is the WORLD frame of the support -/// set). The support direction used to compute the support set is the positive -/// z-axis if the support set has the DEFAULT direction; negative z-axis if it -/// has the INVERTED direction. (In short, a shape's support set is has the -/// DEFAULT direction if the shape is the first shape in a collision pair. It -/// has the INVERTED direction if the shape is the second one in the collision -/// pair). -/// @return an approximation of the set {argmax_{v in shape} v.dot(dir)}, where -/// dir is the support set's support direction. -/// The support set is a plane passing by the origin of the support set frame -/// and supported by the direction dir. As a consequence, any point added to the -/// set is automatically projected onto this plane. -/// @param[in] shape the shape. -/// @param[in/out] support_set of shape. -/// @param[in/out] hint used to initialize the search when shape is a ConvexBase -/// object. -/// @param[in] num_sampled_supports is only used for shapes with smooth -/// non-strictly convex bases like cones and cylinders (their bases are -/// circles). In such a case, if the support direction points to their base, we -/// have to choose which points we want to add to the set. This is not needed -/// for boxes or ConvexBase for example. Indeed, because their support sets are -/// always polygons, we can characterize the entire support set with the -/// vertices of the polygon. -/// @param[in] tol given a point v on the shape, if -/// `max_{p in shape}(p.dot(dir)) - v.dot(dir) <= tol`, where dir is the set's -/// support direction, then v is added to the support set. -/// Otherwise said, if a point p of the shape is at a distance `tol` from the -/// support plane, it is added to the set. Thus, `tol` can be seen as the -/// "thickness" of the support plane. -/// @tparam SupportOptions is a value of the SupportOptions enum. If set to -/// `WithSweptSphere`, the support functions take into account the shapes' swept -/// sphere radii. -template -void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint, - size_t num_sampled_supports = 6, FCL_REAL tol = 1e-3); - -/// @brief Same as @ref getSupportSet(const ShapeBase*, const FCL_REAL, -/// SupportSet&, const int) but also constructs the support set frame from -/// `dir`. -/// @note The support direction `dir` is expressed in the local frame of the -/// shape. -/// @note This function automatically deals with the `direction` of the -/// SupportSet. -template -void getSupportSet(const ShapeBase* shape, const Vec3f& dir, - SupportSet& support_set, int& hint, - size_t num_sampled_supports = 6, FCL_REAL tol = 1e-3) { - support_set.tf.rotation() = constructOrthonormalBasisFromVector(dir); - const Vec3f& support_dir = support_set.getNormal(); - const Vec3f support = getSupport<_SupportOptions>(shape, support_dir, hint); - getSupportSet<_SupportOptions>(shape, support_set, hint, num_sampled_supports, - tol); -} - -/// @brief Triangle support set function. -/// Assumes the support set frame has already been computed. -template -void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set, - int& /*unused*/, ShapeSupportData& /*unused*/, - size_t /*unused*/ num_sampled_supports = 6, - FCL_REAL tol = 1e-3); - -/// @brief Box support set function. -/// Assumes the support set frame has already been computed. -template -void getShapeSupportSet(const Box* box, SupportSet& support_set, - int& /*unused*/, ShapeSupportData& support_data, - size_t /*unused*/ num_sampled_supports = 6, - FCL_REAL tol = 1e-3); - -/// @brief Sphere support set function. -/// Assumes the support set frame has already been computed. -template -void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set, - int& /*unused*/, ShapeSupportData& /*unused*/, - size_t /*unused*/ num_sampled_supports = 6, - FCL_REAL /*unused*/ tol = 1e-3); - -/// @brief Ellipsoid support set function. -/// Assumes the support set frame has already been computed. -template -void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set, - int& /*unused*/, ShapeSupportData& /*unused*/, - size_t /*unused*/ num_sampled_supports = 6, - FCL_REAL /*unused*/ tol = 1e-3); - -/// @brief Capsule support set function. -/// Assumes the support set frame has already been computed. -template -void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set, - int& /*unused*/, ShapeSupportData& /*unused*/, - size_t /*unused*/ num_sampled_supports = 6, - FCL_REAL tol = 1e-3); - -/// @brief Cone support set function. -/// Assumes the support set frame has already been computed. -template -void getShapeSupportSet(const Cone* cone, SupportSet& support_set, - int& /*unused*/, ShapeSupportData& /*unused*/, - size_t num_sampled_supports = 6, FCL_REAL tol = 1e-3); - -/// @brief Cylinder support set function. -/// Assumes the support set frame has already been computed. -template -void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set, - int& /*unused*/, ShapeSupportData& /*unused*/, - size_t num_sampled_supports = 6, FCL_REAL tol = 1e-3); - -/// @brief ConvexBase support set function. -/// Assumes the support set frame has already been computed. -/// @note See @ref LargeConvex and SmallConvex to see how to optimize -/// ConvexBase's support computation. -template -void getShapeSupportSet(const ConvexBase* convex, SupportSet& support_set, - int& hint, ShapeSupportData& support_data, - size_t /*unused*/ num_sampled_supports = 6, - FCL_REAL tol = 1e-3); - -/// @brief Support set function for large ConvexBase (>32 vertices). -/// Assumes the support set frame has already been computed. -template -void getShapeSupportSet(const SmallConvex* convex, SupportSet& support_set, - int& /*unused*/, ShapeSupportData& /*unused*/, - size_t /*unused*/ num_sampled_supports = 6, - FCL_REAL tol = 1e-3); - -/// @brief Support set function for small ConvexBase (<32 vertices). -/// Assumes the support set frame has already been computed. -template -void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set, - int& hint, ShapeSupportData& support_data, - size_t /*unused*/ num_sampled_supports = 6, - FCL_REAL tol = 1e-3); - -/// @brief Computes the convex-hull of support_set. For now, this function is -/// only needed for Box and ConvexBase. -/// @param[in] cloud data which contains the 2d points of the support set which -/// convex-hull we want to compute. -/// @param[out] 2d points of the the support set's convex-hull. -HPP_FCL_DLLAPI void computeSupportSetConvexHull(SupportSet::Polygon& cloud, - SupportSet::Polygon& cvx_hull); - -} // namespace details - -} // namespace fcl -} // namespace hpp - -#endif // HPP_FCL_SUPPORT_FUNCTIONS_H +#include +#include diff --git a/include/hpp/fcl/octree.h b/include/hpp/fcl/octree.h index feec9a7eb..b7f745e79 100644 --- a/include/hpp/fcl/octree.h +++ b/include/hpp/fcl/octree.h @@ -1,342 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * Copyright (c) 2022-2024, Inria - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_OCTREE_H -#define HPP_FCL_OCTREE_H - -#include - -#include -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// @brief Octree is one type of collision geometry which can encode uncertainty -/// information in the sensor data. -class HPP_FCL_DLLAPI OcTree : public CollisionGeometry { - protected: - shared_ptr tree; - - FCL_REAL default_occupancy; - - FCL_REAL occupancy_threshold; - FCL_REAL free_threshold; - - public: - typedef octomap::OcTreeNode OcTreeNode; - - /// @brief construct octree with a given resolution - explicit OcTree(FCL_REAL resolution) - : tree(shared_ptr( - new octomap::OcTree(resolution))) { - default_occupancy = tree->getOccupancyThres(); - - // default occupancy/free threshold is consistent with default setting from - // octomap - occupancy_threshold = tree->getOccupancyThres(); - free_threshold = 0; - } - - /// @brief construct octree from octomap - explicit OcTree(const shared_ptr& tree_) - : tree(tree_) { - default_occupancy = tree->getOccupancyThres(); - - // default occupancy/free threshold is consistent with default setting from - // octomap - occupancy_threshold = tree->getOccupancyThres(); - free_threshold = 0; - } - - ///  \brief Copy constructor - OcTree(const OcTree& other) - : CollisionGeometry(other), - tree(other.tree), - default_occupancy(other.default_occupancy), - occupancy_threshold(other.occupancy_threshold), - free_threshold(other.free_threshold) {} - - /// \brief Clone *this into a new Octree - OcTree* clone() const { return new OcTree(*this); } - - /// \brief Returns the tree associated to the underlying octomap OcTree. - shared_ptr getTree() const { return tree; } - - void exportAsObjFile(const std::string& filename) const; - - /// @brief compute the AABB for the octree in its local coordinate system - void computeLocalAABB() { - typedef Eigen::Matrix Vec3float; - Vec3float max_extent, min_extent; - - octomap::OcTree::iterator it = - tree->begin((unsigned char)tree->getTreeDepth()); - octomap::OcTree::iterator end = tree->end(); - - if (it == end) return; - - { - const octomap::point3d& coord = - it.getCoordinate(); // getCoordinate returns a copy - max_extent = min_extent = Eigen::Map(&coord.x()); - for (++it; it != end; ++it) { - const octomap::point3d& coord = it.getCoordinate(); - const Vec3float pos = Eigen::Map(&coord.x()); - max_extent = max_extent.array().max(pos.array()); - min_extent = min_extent.array().min(pos.array()); - } - } - - // Account for the size of the boxes. - const FCL_REAL resolution = tree->getResolution(); - max_extent.array() += float(resolution / 2.); - min_extent.array() -= float(resolution / 2.); - - aabb_local = AABB(min_extent.cast(), max_extent.cast()); - aabb_center = aabb_local.center(); - aabb_radius = (aabb_local.min_ - aabb_center).norm(); - } - - /// @brief get the bounding volume for the root - AABB getRootBV() const { - FCL_REAL delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2; - - // std::cout << "octree size " << delta << std::endl; - return AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta)); - } - - /// @brief Returns the depth of the octree - unsigned int getTreeDepth() const { return tree->getTreeDepth(); } - - /// @brief Returns the size of the octree - unsigned long size() const { return tree->size(); } - - /// @brief Returns the resolution of the octree - FCL_REAL getResolution() const { return tree->getResolution(); } - - /// @brief get the root node of the octree - OcTreeNode* getRoot() const { return tree->getRoot(); } - - /// @brief whether one node is completely occupied - bool isNodeOccupied(const OcTreeNode* node) const { - // return tree->isNodeOccupied(node); - return node->getOccupancy() >= occupancy_threshold; - } - - /// @brief whether one node is completely free - bool isNodeFree(const OcTreeNode* node) const { - // return false; // default no definitely free node - return node->getOccupancy() <= free_threshold; - } - - /// @brief whether one node is uncertain - bool isNodeUncertain(const OcTreeNode* node) const { - return (!isNodeOccupied(node)) && (!isNodeFree(node)); - } - - /// @brief transform the octree into a bunch of boxes; uncertainty information - /// is kept in the boxes. However, we only keep the occupied boxes (i.e., the - /// boxes whose occupied probability is higher enough). - std::vector toBoxes() const { - std::vector boxes; - boxes.reserve(tree->size() / 2); - for (octomap::OcTree::iterator - it = tree->begin((unsigned char)tree->getTreeDepth()), - end = tree->end(); - it != end; ++it) { - // if(tree->isNodeOccupied(*it)) - if (isNodeOccupied(&*it)) { - FCL_REAL x = it.getX(); - FCL_REAL y = it.getY(); - FCL_REAL z = it.getZ(); - FCL_REAL size = it.getSize(); - FCL_REAL c = (*it).getOccupancy(); - FCL_REAL t = tree->getOccupancyThres(); - - Vec6f box; - box << x, y, z, size, c, t; - boxes.push_back(box); - } - } - return boxes; - } - - /// \brief Returns a byte description of *this - std::vector tobytes() const { - typedef Eigen::Matrix Vec3float; - const size_t total_size = (tree->size() * sizeof(FCL_REAL) * 3) / 2; - std::vector bytes; - bytes.reserve(total_size); - - for (octomap::OcTree::iterator - it = tree->begin((unsigned char)tree->getTreeDepth()), - end = tree->end(); - it != end; ++it) { - const Vec3f box_pos = - Eigen::Map(&it.getCoordinate().x()).cast(); - if (isNodeOccupied(&*it)) - std::copy(box_pos.data(), box_pos.data() + sizeof(FCL_REAL) * 3, - std::back_inserter(bytes)); - } - - return bytes; - } - - /// @brief the threshold used to decide whether one node is occupied, this is - /// NOT the octree occupied_thresold - FCL_REAL getOccupancyThres() const { return occupancy_threshold; } - - /// @brief the threshold used to decide whether one node is free, this is NOT - /// the octree free_threshold - FCL_REAL getFreeThres() const { return free_threshold; } - - FCL_REAL getDefaultOccupancy() const { return default_occupancy; } - - void setCellDefaultOccupancy(FCL_REAL d) { default_occupancy = d; } - - void setOccupancyThres(FCL_REAL d) { occupancy_threshold = d; } - - void setFreeThres(FCL_REAL d) { free_threshold = d; } - - /// @return ptr to child number childIdx of node - OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) { -#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0) - return tree->getNodeChild(node, childIdx); -#else - return node->getChild(childIdx); -#endif - } - - /// @return const ptr to child number childIdx of node - const OcTreeNode* getNodeChild(const OcTreeNode* node, - unsigned int childIdx) const { -#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0) - return tree->getNodeChild(node, childIdx); -#else - return node->getChild(childIdx); -#endif - } - - /// @brief return true if the child at childIdx exists - bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const { -#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0) - return tree->nodeChildExists(node, childIdx); -#else - return node->childExists(childIdx); -#endif - } - - /// @brief return true if node has at least one child - bool nodeHasChildren(const OcTreeNode* node) const { -#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0) - return tree->nodeHasChildren(node); -#else - return node->hasChildren(); -#endif - } - - /// @brief return object type, it is an octree - OBJECT_TYPE getObjectType() const { return OT_OCTREE; } - - /// @brief return node type, it is an octree - NODE_TYPE getNodeType() const { return GEOM_OCTREE; } - - private: - virtual bool isEqual(const CollisionGeometry& _other) const { - const OcTree* other_ptr = dynamic_cast(&_other); - if (other_ptr == nullptr) return false; - const OcTree& other = *other_ptr; - - return (tree.get() == other.tree.get() || toBoxes() == other.toBoxes()) && - default_occupancy == other.default_occupancy && - occupancy_threshold == other.occupancy_threshold && - free_threshold == other.free_threshold; - } - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/// @brief compute the bounding volume of an octree node's i-th child -static inline void computeChildBV(const AABB& root_bv, unsigned int i, - AABB& child_bv) { - if (i & 1) { - child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5; - child_bv.max_[0] = root_bv.max_[0]; - } else { - child_bv.min_[0] = root_bv.min_[0]; - child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5; - } - - if (i & 2) { - child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5; - child_bv.max_[1] = root_bv.max_[1]; - } else { - child_bv.min_[1] = root_bv.min_[1]; - child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5; - } - - if (i & 4) { - child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5; - child_bv.max_[2] = root_bv.max_[2]; - } else { - child_bv.min_[2] = root_bv.min_[2]; - child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5; - } -} - -/// -/// \brief Build an OcTree from a point cloud and a given resolution -/// -/// \param[in] point_cloud The input points to insert in the OcTree -/// \param[in] resolution of the octree. -/// -/// \returns An OcTree that can be used for collision checking and more. -/// -HPP_FCL_DLLAPI OcTreePtr_t -makeOctree(const Eigen::Matrix& point_cloud, - const FCL_REAL resolution); - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/serialization/AABB.h b/include/hpp/fcl/serialization/AABB.h index 85ff64915..b001c4ec3 100644 --- a/include/hpp/fcl/serialization/AABB.h +++ b/include/hpp/fcl/serialization/AABB.h @@ -1,24 +1,2 @@ -// -// Copyright (c) 2021 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_AABB_H -#define HPP_FCL_SERIALIZATION_AABB_H - -#include "hpp/fcl/BV/AABB.h" -#include "hpp/fcl/serialization/fwd.h" - -namespace boost { -namespace serialization { - -template -void serialize(Archive& ar, hpp::fcl::AABB& aabb, - const unsigned int /*version*/) { - ar& make_nvp("min_", aabb.min_); - ar& make_nvp("max_", aabb.max_); -} - -} // namespace serialization -} // namespace boost - -#endif // ifndef HPP_FCL_SERIALIZATION_AABB_H +#include +#include diff --git a/include/hpp/fcl/serialization/BVH_model.h b/include/hpp/fcl/serialization/BVH_model.h index 47e4fb787..c4f65ab50 100644 --- a/include/hpp/fcl/serialization/BVH_model.h +++ b/include/hpp/fcl/serialization/BVH_model.h @@ -1,252 +1,2 @@ -// -// Copyright (c) 2021-2022 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_BVH_MODEL_H -#define HPP_FCL_SERIALIZATION_BVH_MODEL_H - -#include "hpp/fcl/BVH/BVH_model.h" - -#include "hpp/fcl/serialization/fwd.h" -#include "hpp/fcl/serialization/BV_node.h" -#include "hpp/fcl/serialization/BV_splitter.h" -#include "hpp/fcl/serialization/collision_object.h" -#include "hpp/fcl/serialization/memory.h" -#include "hpp/fcl/serialization/triangle.h" - -namespace boost { -namespace serialization { - -namespace internal { -struct BVHModelBaseAccessor : hpp::fcl::BVHModelBase { - typedef hpp::fcl::BVHModelBase Base; - using Base::num_tris_allocated; - using Base::num_vertices_allocated; -}; -} // namespace internal - -template -void save(Archive &ar, const hpp::fcl::BVHModelBase &bvh_model, - const unsigned int /*version*/) { - using namespace hpp::fcl; - if (!(bvh_model.build_state == BVH_BUILD_STATE_PROCESSED || - bvh_model.build_state == BVH_BUILD_STATE_UPDATED) && - (bvh_model.getModelType() == BVH_MODEL_TRIANGLES)) { - HPP_FCL_THROW_PRETTY( - "The BVH model is not in a BVH_BUILD_STATE_PROCESSED or " - "BVH_BUILD_STATE_UPDATED state.\n" - "The BVHModel could not be serialized.", - std::invalid_argument); - } - - ar &make_nvp("base", - boost::serialization::base_object( - bvh_model)); - - ar &make_nvp("num_vertices", bvh_model.num_vertices); - ar &make_nvp("vertices", bvh_model.vertices); - - ar &make_nvp("num_tris", bvh_model.num_tris); - ar &make_nvp("tri_indices", bvh_model.tri_indices); - ar &make_nvp("build_state", bvh_model.build_state); - - ar &make_nvp("prev_vertices", bvh_model.prev_vertices); - - // if(bvh_model.convex) - // { - // const bool has_convex = true; - // ar << make_nvp("has_convex",has_convex); - // } - // else - // { - // const bool has_convex = false; - // ar << make_nvp("has_convex",has_convex); - // } -} - -template -void load(Archive &ar, hpp::fcl::BVHModelBase &bvh_model, - const unsigned int /*version*/) { - using namespace hpp::fcl; - - ar >> make_nvp("base", - boost::serialization::base_object( - bvh_model)); - - ar >> make_nvp("num_vertices", bvh_model.num_vertices); - ar >> make_nvp("vertices", bvh_model.vertices); - - ar >> make_nvp("num_tris", bvh_model.num_tris); - ar >> make_nvp("tri_indices", bvh_model.tri_indices); - ar >> make_nvp("build_state", bvh_model.build_state); - - ar >> make_nvp("prev_vertices", bvh_model.prev_vertices); - - // bool has_convex = true; - // ar >> make_nvp("has_convex",has_convex); -} - -HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::BVHModelBase) - -namespace internal { -template -struct BVHModelAccessor : hpp::fcl::BVHModel { - typedef hpp::fcl::BVHModel Base; - using Base::bvs; - using Base::num_bvs; - using Base::num_bvs_allocated; - using Base::primitive_indices; -}; -} // namespace internal - -template -void serialize(Archive &ar, hpp::fcl::BVHModel &bvh_model, - const unsigned int version) { - split_free(ar, bvh_model, version); -} - -template -void save(Archive &ar, const hpp::fcl::BVHModel &bvh_model_, - const unsigned int /*version*/) { - using namespace hpp::fcl; - typedef internal::BVHModelAccessor Accessor; - typedef BVNode Node; - - const Accessor &bvh_model = reinterpret_cast(bvh_model_); - ar &make_nvp("base", - boost::serialization::base_object(bvh_model)); - - // if(bvh_model.primitive_indices) - // { - // const bool with_primitive_indices = true; - // ar & make_nvp("with_primitive_indices",with_primitive_indices); - // - // int num_primitives = 0; - // switch(bvh_model.getModelType()) - // { - // case BVH_MODEL_TRIANGLES: - // num_primitives = bvh_model.num_tris; - // break; - // case BVH_MODEL_POINTCLOUD: - // num_primitives = bvh_model.num_vertices; - // break; - // default: - // ; - // } - // - // ar & make_nvp("num_primitives",num_primitives); - // if(num_primitives > 0) - // { - // typedef Eigen::Matrix - // AsPrimitiveIndexVector; const Eigen::Map - // primitive_indices_map(reinterpret_cast(bvh_model.primitive_indices),1,num_primitives); ar & - // make_nvp("primitive_indices",primitive_indices_map); - //// ar & - /// make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives)); - // } - // } - // else - // { - // const bool with_primitive_indices = false; - // ar & make_nvp("with_primitive_indices",with_primitive_indices); - // } - // - - if (bvh_model.bvs.get()) { - const bool with_bvs = true; - ar &make_nvp("with_bvs", with_bvs); - ar &make_nvp("num_bvs", bvh_model.num_bvs); - ar &make_nvp( - "bvs", - make_array( - reinterpret_cast(bvh_model.bvs->data()), - sizeof(Node) * - (std::size_t)bvh_model.num_bvs)); // Assuming BVs are POD. - } else { - const bool with_bvs = false; - ar &make_nvp("with_bvs", with_bvs); - } -} - -template -void load(Archive &ar, hpp::fcl::BVHModel &bvh_model_, - const unsigned int /*version*/) { - using namespace hpp::fcl; - typedef internal::BVHModelAccessor Accessor; - typedef BVNode Node; - - Accessor &bvh_model = reinterpret_cast(bvh_model_); - - ar >> make_nvp("base", - boost::serialization::base_object(bvh_model)); - - // bool with_primitive_indices; - // ar >> make_nvp("with_primitive_indices",with_primitive_indices); - // if(with_primitive_indices) - // { - // int num_primitives; - // ar >> make_nvp("num_primitives",num_primitives); - // - // delete[] bvh_model.primitive_indices; - // if(num_primitives > 0) - // { - // bvh_model.primitive_indices = new unsigned int[num_primitives]; - // ar & - // make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives)); - // } - // else - // bvh_model.primitive_indices = NULL; - // } - - bool with_bvs; - ar >> make_nvp("with_bvs", with_bvs); - if (with_bvs) { - unsigned int num_bvs; - ar >> make_nvp("num_bvs", num_bvs); - - if (num_bvs != bvh_model.num_bvs) { - bvh_model.bvs.reset(); - bvh_model.num_bvs = num_bvs; - if (num_bvs > 0) - bvh_model.bvs.reset(new - typename BVHModel::bv_node_vector_t(num_bvs)); - } - if (num_bvs > 0) { - ar >> make_nvp("bvs", - make_array(reinterpret_cast(bvh_model.bvs->data()), - sizeof(Node) * (std::size_t)num_bvs)); - } else - bvh_model.bvs.reset(); - } -} - -} // namespace serialization -} // namespace boost - -namespace hpp { -namespace fcl { - -namespace internal { -template -struct memory_footprint_evaluator<::hpp::fcl::BVHModel> { - static size_t run(const ::hpp::fcl::BVHModel &bvh_model) { - return static_cast(bvh_model.memUsage(false)); - } -}; -} // namespace internal - -} // namespace fcl -} // namespace hpp - -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::AABB>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::OBB>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::RSS>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::OBBRSS>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::kIOS>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::KDOP<16>>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::KDOP<18>>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::KDOP<24>>) - -#endif // ifndef HPP_FCL_SERIALIZATION_BVH_MODEL_H +#include +#include diff --git a/include/hpp/fcl/serialization/BV_node.h b/include/hpp/fcl/serialization/BV_node.h index 78193cda5..a8407a076 100644 --- a/include/hpp/fcl/serialization/BV_node.h +++ b/include/hpp/fcl/serialization/BV_node.h @@ -1,35 +1,2 @@ -// -// Copyright (c) 2021 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_BV_NODE_H -#define HPP_FCL_SERIALIZATION_BV_NODE_H - -#include "hpp/fcl/BV/BV_node.h" - -#include "hpp/fcl/serialization/fwd.h" -#include "hpp/fcl/serialization/OBBRSS.h" - -namespace boost { -namespace serialization { - -template -void serialize(Archive& ar, hpp::fcl::BVNodeBase& node, - const unsigned int /*version*/) { - ar& make_nvp("first_child", node.first_child); - ar& make_nvp("first_primitive", node.first_primitive); - ar& make_nvp("num_primitives", node.num_primitives); -} - -template -void serialize(Archive& ar, hpp::fcl::BVNode& node, - const unsigned int /*version*/) { - ar& make_nvp("base", - boost::serialization::base_object(node)); - ar& make_nvp("bv", node.bv); -} - -} // namespace serialization -} // namespace boost - -#endif // ifndef HPP_FCL_SERIALIZATION_BV_NODE_H +#include +#include diff --git a/include/hpp/fcl/serialization/BV_splitter.h b/include/hpp/fcl/serialization/BV_splitter.h index 24eac7649..7d13844e3 100644 --- a/include/hpp/fcl/serialization/BV_splitter.h +++ b/include/hpp/fcl/serialization/BV_splitter.h @@ -1,62 +1,2 @@ -// -// Copyright (c) 2021 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_BV_SPLITTER_H -#define HPP_FCL_SERIALIZATION_BV_SPLITTER_H - -#include "hpp/fcl/internal/BV_splitter.h" - -#include "hpp/fcl/serialization/fwd.h" - -namespace boost { -namespace serialization { - -namespace internal { -template -struct BVSplitterAccessor : hpp::fcl::BVSplitter { - typedef hpp::fcl::BVSplitter Base; - using Base::split_axis; - using Base::split_method; - using Base::split_value; - using Base::split_vector; - using Base::tri_indices; - using Base::type; - using Base::vertices; -}; -} // namespace internal - -template -void save(Archive &ar, const hpp::fcl::BVSplitter &splitter_, - const unsigned int /*version*/) { - using namespace hpp::fcl; - typedef internal::BVSplitterAccessor Accessor; - const Accessor &splitter = reinterpret_cast(splitter_); - - ar &make_nvp("split_axis", splitter.split_axis); - ar &make_nvp("split_vector", splitter.split_vector); - ar &make_nvp("split_value", splitter.split_value); - ar &make_nvp("type", splitter.type); - ar &make_nvp("split_method", splitter.split_method); -} - -template -void load(Archive &ar, hpp::fcl::BVSplitter &splitter_, - const unsigned int /*version*/) { - using namespace hpp::fcl; - typedef internal::BVSplitterAccessor Accessor; - Accessor &splitter = reinterpret_cast(splitter_); - - ar >> make_nvp("split_axis", splitter.split_axis); - ar >> make_nvp("split_vector", splitter.split_vector); - ar >> make_nvp("split_value", splitter.split_value); - ar >> make_nvp("type", splitter.type); - ar >> make_nvp("split_method", splitter.split_method); - - splitter.vertices = NULL; - splitter.tri_indices = NULL; -} -} // namespace serialization -} // namespace boost - -#endif // ifndef HPP_FCL_SERIALIZATION_BV_SPLITTER_H +#include +#include diff --git a/include/hpp/fcl/serialization/OBB.h b/include/hpp/fcl/serialization/OBB.h index 2c07a44e7..8eebfec17 100644 --- a/include/hpp/fcl/serialization/OBB.h +++ b/include/hpp/fcl/serialization/OBB.h @@ -1,25 +1,2 @@ -// -// Copyright (c) 2021 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_OBB_H -#define HPP_FCL_SERIALIZATION_OBB_H - -#include "hpp/fcl/BV/OBB.h" - -#include "hpp/fcl/serialization/fwd.h" - -namespace boost { -namespace serialization { - -template -void serialize(Archive& ar, hpp::fcl::OBB& bv, const unsigned int /*version*/) { - ar& make_nvp("axes", bv.axes); - ar& make_nvp("To", bv.To); - ar& make_nvp("extent", bv.extent); -} - -} // namespace serialization -} // namespace boost - -#endif // ifndef HPP_FCL_SERIALIZATION_OBB_H +#include +#include diff --git a/include/hpp/fcl/serialization/OBBRSS.h b/include/hpp/fcl/serialization/OBBRSS.h index b19ce3a36..c9673a7f4 100644 --- a/include/hpp/fcl/serialization/OBBRSS.h +++ b/include/hpp/fcl/serialization/OBBRSS.h @@ -1,27 +1,2 @@ -// -// Copyright (c) 2021 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_OBBRSS_H -#define HPP_FCL_SERIALIZATION_OBBRSS_H - -#include "hpp/fcl/BV/OBBRSS.h" - -#include "hpp/fcl/serialization/fwd.h" -#include "hpp/fcl/serialization/OBB.h" -#include "hpp/fcl/serialization/RSS.h" - -namespace boost { -namespace serialization { - -template -void serialize(Archive& ar, hpp::fcl::OBBRSS& bv, - const unsigned int /*version*/) { - ar& make_nvp("obb", bv.obb); - ar& make_nvp("rss", bv.rss); -} - -} // namespace serialization -} // namespace boost - -#endif // ifndef HPP_FCL_SERIALIZATION_OBBRSS_H +#include +#include diff --git a/include/hpp/fcl/serialization/RSS.h b/include/hpp/fcl/serialization/RSS.h index 4f1f7ee75..b7639aa86 100644 --- a/include/hpp/fcl/serialization/RSS.h +++ b/include/hpp/fcl/serialization/RSS.h @@ -1,26 +1,2 @@ -// -// Copyright (c) 2021 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_RSS_H -#define HPP_FCL_SERIALIZATION_RSS_H - -#include "hpp/fcl/BV/RSS.h" - -#include "hpp/fcl/serialization/fwd.h" - -namespace boost { -namespace serialization { - -template -void serialize(Archive& ar, hpp::fcl::RSS& bv, const unsigned int /*version*/) { - ar& make_nvp("axes", bv.axes); - ar& make_nvp("Tr", bv.Tr); - ar& make_nvp("length", make_array(bv.length, 2)); - ar& make_nvp("radius", bv.radius); -} - -} // namespace serialization -} // namespace boost - -#endif // ifndef HPP_FCL_SERIALIZATION_RSS_H +#include +#include diff --git a/include/hpp/fcl/serialization/archive.h b/include/hpp/fcl/serialization/archive.h index 2da3a5db1..0a3f5c188 100644 --- a/include/hpp/fcl/serialization/archive.h +++ b/include/hpp/fcl/serialization/archive.h @@ -1,296 +1,2 @@ -// -// Copyright (c) 2017-2024 CNRS INRIA -// This file was borrowed from the Pinocchio library: -// https://github.com/stack-of-tasks/pinocchio -// - -#ifndef HPP_FCL_SERIALIZATION_ARCHIVE_H -#define HPP_FCL_SERIALIZATION_ARCHIVE_H - -#include "hpp/fcl/fwd.hh" - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#if BOOST_VERSION / 100 % 1000 == 78 && __APPLE__ -// See https://github.com/qcscine/utilities/issues/5#issuecomment-1246897049 for -// further details - -#ifndef BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC -#define DEFINE_BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC -#define BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC -#endif - -#include - -#ifdef DEFINE_BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC -#undef BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC -#endif - -#else -#include -#endif - -#include -#include -#include - -// Handle NAN inside TXT or XML archives -#include - -namespace hpp { -namespace fcl { -namespace serialization { - -/// -/// \brief Loads an object from a TXT file. -/// -/// \tparam T Type of the object to deserialize. -/// -/// \param[out] object Object in which the loaded data are copied. -/// \param[in] filename Name of the file containing the serialized data. -/// -template -inline void loadFromText(T& object, const std::string& filename) { - std::ifstream ifs(filename.c_str()); - if (ifs) { - std::locale const new_loc(ifs.getloc(), - new boost::math::nonfinite_num_get); - ifs.imbue(new_loc); - boost::archive::text_iarchive ia(ifs, boost::archive::no_codecvt); - ia >> object; - } else { - const std::string exception_message(filename + - " does not seem to be a valid file."); - throw std::invalid_argument(exception_message); - } -} - -/// -/// \brief Saves an object inside a TXT file. -/// -/// \tparam T Type of the object to deserialize. -/// -/// \param[in] object Object in which the loaded data are copied. -/// \param[in] filename Name of the file containing the serialized data. -/// -template -inline void saveToText(const T& object, const std::string& filename) { - std::ofstream ofs(filename.c_str()); - if (ofs) { - boost::archive::text_oarchive oa(ofs); - oa & object; - } else { - const std::string exception_message(filename + - " does not seem to be a valid file."); - throw std::invalid_argument(exception_message); - } -} - -/// -/// \brief Loads an object from a std::stringstream. -/// -/// \tparam T Type of the object to deserialize. -/// -/// \param[out] object Object in which the loaded data are copied. -/// \param[in] is string stream constaining the serialized content of the -/// object. -/// -template -inline void loadFromStringStream(T& object, std::istringstream& is) { - std::locale const new_loc(is.getloc(), - new boost::math::nonfinite_num_get); - is.imbue(new_loc); - boost::archive::text_iarchive ia(is, boost::archive::no_codecvt); - ia >> object; -} - -/// -/// \brief Saves an object inside a std::stringstream. -/// -/// \tparam T Type of the object to deserialize. -/// -/// \param[in] object Object in which the loaded data are copied. -/// \param[out] ss String stream constaining the serialized content of the -/// object. -/// -template -inline void saveToStringStream(const T& object, std::stringstream& ss) { - boost::archive::text_oarchive oa(ss); - oa & object; -} - -/// -/// \brief Loads an object from a std::string -/// -/// \tparam T Type of the object to deserialize. -/// -/// \param[out] object Object in which the loaded data are copied. -/// \param[in] str string constaining the serialized content of the object. -/// -template -inline void loadFromString(T& object, const std::string& str) { - std::istringstream is(str); - loadFromStringStream(object, is); -} - -/// -/// \brief Saves an object inside a std::string -/// -/// \tparam T Type of the object to deserialize. -/// -/// \param[in] object Object in which the loaded data are copied. -/// -/// \returns a string constaining the serialized content of the object. -/// -template -inline std::string saveToString(const T& object) { - std::stringstream ss; - saveToStringStream(object, ss); - return ss.str(); -} - -/// -/// \brief Loads an object from a XML file. -/// -/// \tparam T Type of the object to deserialize. -/// -/// \param[out] object Object in which the loaded data are copied. -/// \param[in] filename Name of the file containing the serialized data. -/// \param[in] tag_name XML Tag for the given object. -/// -template -inline void loadFromXML(T& object, const std::string& filename, - const std::string& tag_name) { - if (filename.empty()) { - HPP_FCL_THROW_PRETTY("Tag name should not be empty.", - std::invalid_argument); - } - - std::ifstream ifs(filename.c_str()); - if (ifs) { - std::locale const new_loc(ifs.getloc(), - new boost::math::nonfinite_num_get); - ifs.imbue(new_loc); - boost::archive::xml_iarchive ia(ifs, boost::archive::no_codecvt); - ia >> boost::serialization::make_nvp(tag_name.c_str(), object); - } else { - const std::string exception_message(filename + - " does not seem to be a valid file."); - throw std::invalid_argument(exception_message); - } -} - -/// -/// \brief Saves an object inside a XML file. -/// -/// \tparam T Type of the object to deserialize. -/// -/// \param[in] object Object in which the loaded data are copied. -/// \param[in] filename Name of the file containing the serialized data. -/// \param[in] tag_name XML Tag for the given object. -/// -template -inline void saveToXML(const T& object, const std::string& filename, - const std::string& tag_name) { - if (filename.empty()) { - HPP_FCL_THROW_PRETTY("Tag name should not be empty.", - std::invalid_argument); - } - - std::ofstream ofs(filename.c_str()); - if (ofs) { - boost::archive::xml_oarchive oa(ofs); - oa& boost::serialization::make_nvp(tag_name.c_str(), object); - } else { - const std::string exception_message(filename + - " does not seem to be a valid file."); - throw std::invalid_argument(exception_message); - } -} - -/// -/// \brief Loads an object from a binary file. -/// -/// \tparam T Type of the object to deserialize. -/// -/// \param[out] object Object in which the loaded data are copied. -/// \param[in] filename Name of the file containing the serialized data. -/// -template -inline void loadFromBinary(T& object, const std::string& filename) { - std::ifstream ifs(filename.c_str(), std::ios::binary); - if (ifs) { - boost::archive::binary_iarchive ia(ifs); - ia >> object; - } else { - const std::string exception_message(filename + - " does not seem to be a valid file."); - throw std::invalid_argument(exception_message); - } -} - -/// -/// \brief Saves an object inside a binary file. -/// -/// \tparam T Type of the object to deserialize. -/// -/// \param[in] object Object in which the loaded data are copied. -/// \param[in] filename Name of the file containing the serialized data. -/// -template -void saveToBinary(const T& object, const std::string& filename) { - std::ofstream ofs(filename.c_str(), std::ios::binary); - if (ofs) { - boost::archive::binary_oarchive oa(ofs); - oa & object; - } else { - const std::string exception_message(filename + - " does not seem to be a valid file."); - throw std::invalid_argument(exception_message); - } -} - -/// -/// \brief Loads an object from a binary buffer. -/// -/// \tparam T Type of the object to deserialize. -/// -/// \param[out] object Object in which the loaded data are copied. -/// \param[in] buffer Input buffer containing the serialized data. -/// -template -inline void loadFromBuffer(T& object, boost::asio::streambuf& buffer) { - boost::archive::binary_iarchive ia(buffer); - ia >> object; -} - -/// -/// \brief Saves an object to a binary buffer. -/// -/// \tparam T Type of the object to serialize. -/// -/// \param[in] object Object in which the loaded data are copied. -/// \param[out] buffer Output buffer containing the serialized data. -/// -template -void saveToBuffer(const T& object, boost::asio::streambuf& buffer) { - boost::archive::binary_oarchive oa(buffer); - oa & object; -} - -} // namespace serialization -} // namespace fcl -} // namespace hpp - -#endif // ifndef HPP_FCL_SERIALIZATION_ARCHIVE_H +#include +#include diff --git a/include/hpp/fcl/serialization/collision_data.h b/include/hpp/fcl/serialization/collision_data.h index e32a54330..9ac4f985d 100644 --- a/include/hpp/fcl/serialization/collision_data.h +++ b/include/hpp/fcl/serialization/collision_data.h @@ -1,184 +1,2 @@ -// -// Copyright (c) 2021 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_COLLISION_DATA_H -#define HPP_FCL_SERIALIZATION_COLLISION_DATA_H - -#include "hpp/fcl/collision_data.h" -#include "hpp/fcl/serialization/fwd.h" - -namespace boost { -namespace serialization { - -template -void save(Archive& ar, const hpp::fcl::Contact& contact, - const unsigned int /*version*/) { - ar& make_nvp("b1", contact.b1); - ar& make_nvp("b2", contact.b2); - ar& make_nvp("normal", contact.normal); - ar& make_nvp("nearest_points", contact.nearest_points); - ar& make_nvp("pos", contact.pos); - ar& make_nvp("penetration_depth", contact.penetration_depth); -} - -template -void load(Archive& ar, hpp::fcl::Contact& contact, - const unsigned int /*version*/) { - ar >> make_nvp("b1", contact.b1); - ar >> make_nvp("b2", contact.b2); - ar >> make_nvp("normal", contact.normal); - std::array nearest_points; - ar >> make_nvp("nearest_points", nearest_points); - contact.nearest_points[0] = nearest_points[0]; - contact.nearest_points[1] = nearest_points[1]; - ar >> make_nvp("pos", contact.pos); - ar >> make_nvp("penetration_depth", contact.penetration_depth); - contact.o1 = NULL; - contact.o2 = NULL; -} - -HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::Contact) - -template -void serialize(Archive& ar, hpp::fcl::QueryRequest& query_request, - const unsigned int /*version*/) { - ar& make_nvp("gjk_initial_guess", query_request.gjk_initial_guess); - // TODO: use gjk_initial_guess instead - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - ar& make_nvp("enable_cached_gjk_guess", - query_request.enable_cached_gjk_guess); - HPP_FCL_COMPILER_DIAGNOSTIC_POP - ar& make_nvp("cached_gjk_guess", query_request.cached_gjk_guess); - ar& make_nvp("cached_support_func_guess", - query_request.cached_support_func_guess); - ar& make_nvp("gjk_max_iterations", query_request.gjk_max_iterations); - ar& make_nvp("gjk_tolerance", query_request.gjk_tolerance); - ar& make_nvp("gjk_variant", query_request.gjk_variant); - ar& make_nvp("gjk_convergence_criterion", - query_request.gjk_convergence_criterion); - ar& make_nvp("gjk_convergence_criterion_type", - query_request.gjk_convergence_criterion_type); - ar& make_nvp("epa_max_iterations", query_request.epa_max_iterations); - ar& make_nvp("epa_tolerance", query_request.epa_tolerance); - ar& make_nvp("collision_distance_threshold", - query_request.collision_distance_threshold); - ar& make_nvp("enable_timings", query_request.enable_timings); -} - -template -void serialize(Archive& ar, hpp::fcl::QueryResult& query_result, - const unsigned int /*version*/) { - ar& make_nvp("cached_gjk_guess", query_result.cached_gjk_guess); - ar& make_nvp("cached_support_func_guess", - query_result.cached_support_func_guess); -} - -template -void serialize(Archive& ar, hpp::fcl::CollisionRequest& collision_request, - const unsigned int /*version*/) { - ar& make_nvp("base", - boost::serialization::base_object( - collision_request)); - ar& make_nvp("num_max_contacts", collision_request.num_max_contacts); - ar& make_nvp("enable_contact", collision_request.enable_contact); - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - ar& make_nvp("enable_distance_lower_bound", - collision_request.enable_distance_lower_bound); - HPP_FCL_COMPILER_DIAGNOSTIC_POP - ar& make_nvp("security_margin", collision_request.security_margin); - ar& make_nvp("break_distance", collision_request.break_distance); - ar& make_nvp("distance_upper_bound", collision_request.distance_upper_bound); -} - -template -void save(Archive& ar, const hpp::fcl::CollisionResult& collision_result, - const unsigned int /*version*/) { - ar& make_nvp("base", boost::serialization::base_object( - collision_result)); - ar& make_nvp("contacts", collision_result.getContacts()); - ar& make_nvp("distance_lower_bound", collision_result.distance_lower_bound); - ar& make_nvp("nearest_points", collision_result.nearest_points); - ar& make_nvp("normal", collision_result.normal); -} - -template -void load(Archive& ar, hpp::fcl::CollisionResult& collision_result, - const unsigned int /*version*/) { - ar >> - make_nvp("base", boost::serialization::base_object( - collision_result)); - std::vector contacts; - ar >> make_nvp("contacts", contacts); - collision_result.clear(); - for (size_t k = 0; k < contacts.size(); ++k) - collision_result.addContact(contacts[k]); - ar >> make_nvp("distance_lower_bound", collision_result.distance_lower_bound); - std::array nearest_points; - ar >> make_nvp("nearest_points", nearest_points); - collision_result.nearest_points[0] = nearest_points[0]; - collision_result.nearest_points[1] = nearest_points[1]; - ar >> make_nvp("normal", collision_result.normal); -} - -HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::CollisionResult) - -template -void serialize(Archive& ar, hpp::fcl::DistanceRequest& distance_request, - const unsigned int /*version*/) { - ar& make_nvp("base", - boost::serialization::base_object( - distance_request)); - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - ar& make_nvp("enable_nearest_points", distance_request.enable_nearest_points); - HPP_FCL_COMPILER_DIAGNOSTIC_POP - ar& make_nvp("enable_signed_distance", - distance_request.enable_signed_distance); - ar& make_nvp("rel_err", distance_request.rel_err); - ar& make_nvp("abs_err", distance_request.abs_err); -} - -template -void save(Archive& ar, const hpp::fcl::DistanceResult& distance_result, - const unsigned int /*version*/) { - ar& make_nvp("base", boost::serialization::base_object( - distance_result)); - ar& make_nvp("min_distance", distance_result.min_distance); - ar& make_nvp("nearest_points", distance_result.nearest_points); - ar& make_nvp("normal", distance_result.normal); - ar& make_nvp("b1", distance_result.b1); - ar& make_nvp("b2", distance_result.b2); -} - -template -void load(Archive& ar, hpp::fcl::DistanceResult& distance_result, - const unsigned int /*version*/) { - ar >> - make_nvp("base", boost::serialization::base_object( - distance_result)); - ar >> make_nvp("min_distance", distance_result.min_distance); - std::array nearest_points; - ar >> make_nvp("nearest_points", nearest_points); - distance_result.nearest_points[0] = nearest_points[0]; - distance_result.nearest_points[1] = nearest_points[1]; - ar >> make_nvp("normal", distance_result.normal); - ar >> make_nvp("b1", distance_result.b1); - ar >> make_nvp("b2", distance_result.b2); - distance_result.o1 = NULL; - distance_result.o2 = NULL; -} - -HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::DistanceResult) - -} // namespace serialization -} // namespace boost - -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::CollisionRequest) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::CollisionResult) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::DistanceRequest) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::DistanceResult) - -#endif // ifndef HPP_FCL_SERIALIZATION_COLLISION_DATA_H +#include +#include diff --git a/include/hpp/fcl/serialization/collision_object.h b/include/hpp/fcl/serialization/collision_object.h index 018728cf5..782976870 100644 --- a/include/hpp/fcl/serialization/collision_object.h +++ b/include/hpp/fcl/serialization/collision_object.h @@ -1,95 +1,2 @@ -// -// Copyright (c) 2021 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_COLLISION_OBJECT_H -#define HPP_FCL_SERIALIZATION_COLLISION_OBJECT_H - -#include "hpp/fcl/collision_object.h" - -#include "hpp/fcl/serialization/fwd.h" -#include "hpp/fcl/serialization/AABB.h" - -namespace boost { -namespace serialization { - -template -void save(Archive& ar, const hpp::fcl::CollisionGeometry& collision_geometry, - const unsigned int /*version*/) { - ar& make_nvp("aabb_center", collision_geometry.aabb_center); - ar& make_nvp("aabb_radius", collision_geometry.aabb_radius); - ar& make_nvp("aabb_local", collision_geometry.aabb_local); - ar& make_nvp("cost_density", collision_geometry.cost_density); - ar& make_nvp("threshold_occupied", collision_geometry.threshold_occupied); - ar& make_nvp("threshold_free", collision_geometry.threshold_free); -} - -template -void load(Archive& ar, hpp::fcl::CollisionGeometry& collision_geometry, - const unsigned int /*version*/) { - ar >> make_nvp("aabb_center", collision_geometry.aabb_center); - ar >> make_nvp("aabb_radius", collision_geometry.aabb_radius); - ar >> make_nvp("aabb_local", collision_geometry.aabb_local); - ar >> make_nvp("cost_density", collision_geometry.cost_density); - ar >> make_nvp("threshold_occupied", collision_geometry.threshold_occupied); - ar >> make_nvp("threshold_free", collision_geometry.threshold_free); - collision_geometry.user_data = NULL; // no way to recover this -} - -HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::CollisionGeometry) - -} // namespace serialization -} // namespace boost - -namespace hpp { -namespace fcl { - -// fwd declaration -template -class HeightField; - -template -class Convex; - -struct OBB; -struct OBBRSS; -class AABB; - -class OcTree; -class Box; -class Sphere; -class Ellipsoid; -class Capsule; -class Cone; -class TriangleP; -class Cylinder; -class Halfspace; -class Plane; - -namespace serialization { -template <> -struct register_type { - template - static void on(Archive& ar) { - ar.template register_type(); - ar.template register_type(); - ar.template register_type(); - ar.template register_type(); - ar.template register_type(); - ar.template register_type(); - ar.template register_type(); - ar.template register_type(); - ar.template register_type(); - ar.template register_type(); - ar.template register_type>(); - ar.template register_type>(); - ar.template register_type>(); - ar.template register_type>(); - ; - } -}; -} // namespace serialization -} // namespace fcl -} // namespace hpp - -#endif // ifndef HPP_FCL_SERIALIZATION_COLLISION_OBJECT_H +#include +#include diff --git a/include/hpp/fcl/serialization/contact_patch.h b/include/hpp/fcl/serialization/contact_patch.h index 71d984ca9..dca0506ff 100644 --- a/include/hpp/fcl/serialization/contact_patch.h +++ b/include/hpp/fcl/serialization/contact_patch.h @@ -1,87 +1,2 @@ -// -// Copyright (c) 2024 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_CONTACT_PATCH_H -#define HPP_FCL_SERIALIZATION_CONTACT_PATCH_H - -#include "hpp/fcl/collision_data.h" -#include "hpp/fcl/serialization/fwd.h" -#include "hpp/fcl/serialization/transform.h" - -namespace boost { -namespace serialization { - -template -void serialize(Archive& ar, hpp::fcl::ContactPatch& contact_patch, - const unsigned int /*version*/) { - using namespace hpp::fcl; - typedef Eigen::Matrix PolygonPoints; - - size_t patch_size = contact_patch.size(); - ar& make_nvp("patch_size", patch_size); - if (patch_size > 0) { - if (Archive::is_loading::value) { - contact_patch.points().resize(patch_size); - } - Eigen::Map points_map( - reinterpret_cast(contact_patch.points().data()), 2, - static_cast(patch_size)); - ar& make_nvp("points", points_map); - } - - ar& make_nvp("penetration_depth", contact_patch.penetration_depth); - ar& make_nvp("direction", contact_patch.direction); - ar& make_nvp("tf", contact_patch.tf); -} - -template -void serialize(Archive& ar, hpp::fcl::ContactPatchRequest& request, - const unsigned int /*version*/) { - using namespace hpp::fcl; - - ar& make_nvp("max_num_patch", request.max_num_patch); - - size_t num_samples_curved_shapes = request.getNumSamplesCurvedShapes(); - FCL_REAL patch_tolerance = request.getPatchTolerance(); - ar& make_nvp("num_samples_curved_shapes", num_samples_curved_shapes); - ar& make_nvp("patch_tolerance", num_samples_curved_shapes); - - if (Archive::is_loading::value) { - request.setNumSamplesCurvedShapes(num_samples_curved_shapes); - request.setPatchTolerance(patch_tolerance); - } -} - -template -void serialize(Archive& ar, hpp::fcl::ContactPatchResult& result, - const unsigned int /*version*/) { - using namespace hpp::fcl; - - size_t num_patches = result.numContactPatches(); - ar& make_nvp("num_patches", num_patches); - - std::vector patches; - patches.resize(num_patches); - if (Archive::is_loading::value) { - ar& make_nvp("patches", patches); - - const ContactPatchRequest request(num_patches); - result.set(request); - for (size_t i = 0; i < num_patches; ++i) { - ContactPatch& patch = result.getUnusedContactPatch(); - patch = patches[i]; - } - } else { - patches.clear(); - for (size_t i = 0; i < num_patches; ++i) { - patches.emplace_back(result.getContactPatch(i)); - } - ar& make_nvp("patches", patches); - } -} - -} // namespace serialization -} // namespace boost - -#endif // HPP_FCL_SERIALIZATION_CONTACT_PATCH_H +#include +#include diff --git a/include/hpp/fcl/serialization/convex.h b/include/hpp/fcl/serialization/convex.h index 7bbf37b5d..7000a3e18 100644 --- a/include/hpp/fcl/serialization/convex.h +++ b/include/hpp/fcl/serialization/convex.h @@ -1,169 +1,2 @@ -// -// Copyright (c) 2022-2024 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_CONVEX_H -#define HPP_FCL_SERIALIZATION_CONVEX_H - -#include "hpp/fcl/shape/convex.h" - -#include "hpp/fcl/serialization/fwd.h" -#include "hpp/fcl/serialization/geometric_shapes.h" -#include "hpp/fcl/serialization/memory.h" -#include "hpp/fcl/serialization/triangle.h" -#include "hpp/fcl/serialization/quadrilateral.h" - -namespace boost { -namespace serialization { - -namespace internal { -struct ConvexBaseAccessor : hpp::fcl::ConvexBase { - typedef hpp::fcl::ConvexBase Base; -}; - -} // namespace internal - -template -void serialize(Archive& ar, hpp::fcl::ConvexBase& convex_base, - const unsigned int /*version*/) { - using namespace hpp::fcl; - - ar& make_nvp("base", boost::serialization::base_object( - convex_base)); - - const unsigned int num_points_previous = convex_base.num_points; - ar& make_nvp("num_points", convex_base.num_points); - - const unsigned int num_normals_and_offsets_previous = - convex_base.num_normals_and_offsets; - ar& make_nvp("num_normals_and_offsets", convex_base.num_normals_and_offsets); - - const int num_warm_start_supports_previous = - static_cast(convex_base.support_warm_starts.points.size()); - assert(num_warm_start_supports_previous == - static_cast(convex_base.support_warm_starts.indices.size())); - int num_warm_start_supports = num_warm_start_supports_previous; - ar& make_nvp("num_warm_start_supports", num_warm_start_supports); - - if (Archive::is_loading::value) { - if (num_points_previous != convex_base.num_points) { - convex_base.points.reset(); - if (convex_base.num_points > 0) - convex_base.points.reset( - new std::vector(convex_base.num_points)); - } - - if (num_normals_and_offsets_previous != - convex_base.num_normals_and_offsets) { - convex_base.normals.reset(); - convex_base.offsets.reset(); - if (convex_base.num_normals_and_offsets > 0) { - convex_base.normals.reset( - new std::vector(convex_base.num_normals_and_offsets)); - convex_base.offsets.reset( - new std::vector(convex_base.num_normals_and_offsets)); - } - } - - if (num_warm_start_supports_previous != num_warm_start_supports) { - convex_base.support_warm_starts.points.resize( - static_cast(num_warm_start_supports)); - convex_base.support_warm_starts.indices.resize( - static_cast(num_warm_start_supports)); - } - } - - typedef Eigen::Matrix MatrixPoints; - if (convex_base.num_points > 0) { - Eigen::Map points_map( - reinterpret_cast(convex_base.points->data()), 3, - convex_base.num_points); - ar& make_nvp("points", points_map); - } - - typedef Eigen::Matrix VecOfReals; - if (convex_base.num_normals_and_offsets > 0) { - Eigen::Map normals_map( - reinterpret_cast(convex_base.normals->data()), 3, - convex_base.num_normals_and_offsets); - ar& make_nvp("normals", normals_map); - - Eigen::Map offsets_map( - reinterpret_cast(convex_base.offsets->data()), 1, - convex_base.num_normals_and_offsets); - ar& make_nvp("offsets", offsets_map); - } - - typedef Eigen::Matrix VecOfInts; - if (num_warm_start_supports > 0) { - Eigen::Map warm_start_support_points_map( - reinterpret_cast( - convex_base.support_warm_starts.points.data()), - 3, num_warm_start_supports); - ar& make_nvp("warm_start_support_points", warm_start_support_points_map); - - Eigen::Map warm_start_support_indices_map( - reinterpret_cast(convex_base.support_warm_starts.indices.data()), - 1, num_warm_start_supports); - ar& make_nvp("warm_start_support_indices", warm_start_support_indices_map); - } - - ar& make_nvp("center", convex_base.center); - // We don't save neighbors as they will be computed directly by calling - // fillNeighbors. -} - -namespace internal { -template -struct ConvexAccessor : hpp::fcl::Convex { - typedef hpp::fcl::Convex Base; - using Base::fillNeighbors; -}; - -} // namespace internal - -template -void serialize(Archive& ar, hpp::fcl::Convex& convex_, - const unsigned int /*version*/) { - using namespace hpp::fcl; - typedef internal::ConvexAccessor Accessor; - - Accessor& convex = reinterpret_cast(convex_); - ar& make_nvp("base", boost::serialization::base_object(convex_)); - - const unsigned int num_polygons_previous = convex.num_polygons; - ar& make_nvp("num_polygons", convex.num_polygons); - - if (Archive::is_loading::value) { - if (num_polygons_previous != convex.num_polygons) { - convex.polygons.reset(new std::vector(convex.num_polygons)); - } - } - - ar& make_array(convex.polygons->data(), convex.num_polygons); - - if (Archive::is_loading::value) convex.fillNeighbors(); -} - -} // namespace serialization -} // namespace boost - -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(hpp::fcl::Convex) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(hpp::fcl::Convex) - -namespace hpp { -namespace fcl { - -// namespace internal { -// template -// struct memory_footprint_evaluator< ::hpp::fcl::BVHModel > { -// static size_t run(const ::hpp::fcl::BVHModel &bvh_model) { -// return static_cast(bvh_model.memUsage(false)); -// } -// }; -// } // namespace internal - -} // namespace fcl -} // namespace hpp - -#endif // ifndef HPP_FCL_SERIALIZATION_CONVEX_H +#include +#include diff --git a/include/hpp/fcl/serialization/eigen.h b/include/hpp/fcl/serialization/eigen.h index fa7c49dc9..e5c9faaa7 100644 --- a/include/hpp/fcl/serialization/eigen.h +++ b/include/hpp/fcl/serialization/eigen.h @@ -1,116 +1,2 @@ -// -// Copyright (c) 2017-2021 CNRS INRIA -// - -/* - Code adapted from Pinocchio and - https://gist.githubusercontent.com/mtao/5798888/raw/5be9fa9b66336c166dba3a92c0e5b69ffdb81501/eigen_boost_serialization.hpp - Copyright (c) 2015 Michael Tao -*/ - -#ifndef HPP_FCL_SERIALIZATION_EIGEN_H -#define HPP_FCL_SERIALIZATION_EIGEN_H - -#ifndef HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION - -#include - -#include -#include -#include - -// Workaround a bug in GCC >= 7 and C++17 -// ref. https://gitlab.com/libeigen/eigen/-/issues/1676 -#ifdef __GNUC__ -#if __GNUC__ >= 7 && __cplusplus >= 201703L -namespace boost { -namespace serialization { -struct U; -} -} // namespace boost -namespace Eigen { -namespace internal { -template <> -struct traits { - enum { Flags = 0 }; -}; -} // namespace internal -} // namespace Eigen -#endif -#endif - -namespace boost { -namespace serialization { - -template -void save(Archive& ar, - const Eigen::Matrix& m, - const unsigned int /*version*/) { - Eigen::DenseIndex rows(m.rows()), cols(m.cols()); - if (Rows == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(rows); - if (Cols == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(cols); - ar& make_nvp("data", make_array(m.data(), (size_t)m.size())); -} - -template -void load(Archive& ar, - Eigen::Matrix& m, - const unsigned int /*version*/) { - Eigen::DenseIndex rows = Rows, cols = Cols; - if (Rows == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(rows); - if (Cols == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(cols); - m.resize(rows, cols); - ar >> make_nvp("data", make_array(m.data(), (size_t)m.size())); -} - -template -void serialize(Archive& ar, - Eigen::Matrix& m, - const unsigned int version) { - split_free(ar, m, version); -} - -template -void save(Archive& ar, - const Eigen::Map& m, - const unsigned int /*version*/) { - Eigen::DenseIndex rows(m.rows()), cols(m.cols()); - if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic) - ar& BOOST_SERIALIZATION_NVP(rows); - if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic) - ar& BOOST_SERIALIZATION_NVP(cols); - ar& make_nvp("data", make_array(m.data(), (size_t)m.size())); -} - -template -void load(Archive& ar, Eigen::Map& m, - const unsigned int /*version*/) { - Eigen::DenseIndex rows = PlainObjectBase::RowsAtCompileTime, - cols = PlainObjectBase::ColsAtCompileTime; - if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic) - ar >> BOOST_SERIALIZATION_NVP(rows); - if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic) - ar >> BOOST_SERIALIZATION_NVP(cols); - m.resize(rows, cols); - ar >> make_nvp("data", make_array(m.data(), (size_t)m.size())); -} - -template -void serialize(Archive& ar, - Eigen::Map& m, - const unsigned int version) { - split_free(ar, m, version); -} - -} // namespace serialization -} // namespace boost -// -#endif // ifned HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION - -#endif // ifndef HPP_FCL_SERIALIZATION_EIGEN_H +#include +#include diff --git a/include/hpp/fcl/serialization/fwd.h b/include/hpp/fcl/serialization/fwd.h index abb1943fc..69e442f46 100644 --- a/include/hpp/fcl/serialization/fwd.h +++ b/include/hpp/fcl/serialization/fwd.h @@ -1,117 +1,2 @@ -// -// Copyright (c) 2021-2024 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_FWD_H -#define HPP_FCL_SERIALIZATION_FWD_H - -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "hpp/fcl/fwd.hh" -#include "hpp/fcl/serialization/eigen.h" - -#define HPP_FCL_SERIALIZATION_SPLIT(Type) \ - template \ - void serialize(Archive& ar, Type& value, const unsigned int version) { \ - split_free(ar, value, version); \ - } - -#define HPP_FCL_SERIALIZATION_DECLARE_EXPORT(T) \ - BOOST_CLASS_EXPORT_KEY(T) \ - namespace boost { \ - namespace archive { \ - namespace detail { \ - namespace extra_detail { \ - template <> \ - struct init_guid { \ - static guid_initializer const& g; \ - }; \ - } \ - } \ - } \ - } \ - /**/ - -#define HPP_FCL_SERIALIZATION_DEFINE_EXPORT(T) \ - namespace boost { \ - namespace archive { \ - namespace detail { \ - namespace extra_detail { \ - guid_initializer const& init_guid::g = \ - ::boost::serialization::singleton< \ - guid_initializer >::get_mutable_instance() \ - .export_guid(); \ - } \ - } \ - } \ - } \ - /**/ - -namespace hpp { -namespace fcl { -namespace serialization { -namespace detail { - -template -struct init_cast_register {}; - -template -struct cast_register_initializer { - void init(std::true_type) const { - boost::serialization::void_cast_register(); - } - - void init(std::false_type) const {} - - cast_register_initializer const& init() const { - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - _Pragma("GCC diagnostic ignored \"-Wconversion\"") - BOOST_STATIC_WARNING((std::is_base_of::value)); - HPP_FCL_COMPILER_DIAGNOSTIC_POP - init(std::is_base_of()); - return *this; - } -}; - -} // namespace detail - -template -struct register_type { - template - static void on(Archive& /*ar*/) {} -}; -} // namespace serialization -} // namespace fcl -} // namespace hpp - -#define HPP_FCL_SERIALIZATION_CAST_REGISTER(Derived, Base) \ - namespace hpp { \ - namespace fcl { \ - namespace serialization { \ - namespace detail { \ - template <> \ - struct init_cast_register { \ - static cast_register_initializer const& g; \ - }; \ - cast_register_initializer const& init_cast_register< \ - Derived, Base>::g = \ - ::boost::serialization::singleton< \ - cast_register_initializer >::get_mutable_instance() \ - .init(); \ - } \ - } \ - } \ - } - -#endif // ifndef HPP_FCL_SERIALIZATION_FWD_H +#include +#include diff --git a/include/hpp/fcl/serialization/geometric_shapes.h b/include/hpp/fcl/serialization/geometric_shapes.h index 9122a2011..d7679f265 100644 --- a/include/hpp/fcl/serialization/geometric_shapes.h +++ b/include/hpp/fcl/serialization/geometric_shapes.h @@ -1,123 +1,2 @@ -// -// Copyright (c) 2021-2024 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_GEOMETRIC_SHAPES_H -#define HPP_FCL_SERIALIZATION_GEOMETRIC_SHAPES_H - -#include "hpp/fcl/shape/geometric_shapes.h" -#include "hpp/fcl/serialization/fwd.h" -#include "hpp/fcl/serialization/collision_object.h" - -namespace boost { -namespace serialization { - -template -void serialize(Archive& ar, hpp::fcl::ShapeBase& shape_base, - const unsigned int /*version*/) { - ar& make_nvp("base", - boost::serialization::base_object( - shape_base)); - ::hpp::fcl::FCL_REAL radius = shape_base.getSweptSphereRadius(); - ar& make_nvp("swept_sphere_radius", radius); - - if (Archive::is_loading::value) { - shape_base.setSweptSphereRadius(radius); - } -} - -template -void serialize(Archive& ar, hpp::fcl::TriangleP& triangle, - const unsigned int /*version*/) { - ar& make_nvp( - "base", boost::serialization::base_object(triangle)); - ar& make_nvp("a", triangle.a); - ar& make_nvp("b", triangle.b); - ar& make_nvp("c", triangle.c); -} - -template -void serialize(Archive& ar, hpp::fcl::Box& box, - const unsigned int /*version*/) { - ar& make_nvp("base", - boost::serialization::base_object(box)); - ar& make_nvp("halfSide", box.halfSide); -} - -template -void serialize(Archive& ar, hpp::fcl::Sphere& sphere, - const unsigned int /*version*/) { - ar& make_nvp("base", - boost::serialization::base_object(sphere)); - ar& make_nvp("radius", sphere.radius); -} - -template -void serialize(Archive& ar, hpp::fcl::Ellipsoid& ellipsoid, - const unsigned int /*version*/) { - ar& make_nvp("base", boost::serialization::base_object( - ellipsoid)); - ar& make_nvp("radii", ellipsoid.radii); -} - -template -void serialize(Archive& ar, hpp::fcl::Capsule& capsule, - const unsigned int /*version*/) { - ar& make_nvp("base", - boost::serialization::base_object(capsule)); - ar& make_nvp("radius", capsule.radius); - ar& make_nvp("halfLength", capsule.halfLength); -} - -template -void serialize(Archive& ar, hpp::fcl::Cone& cone, - const unsigned int /*version*/) { - ar& make_nvp("base", - boost::serialization::base_object(cone)); - ar& make_nvp("radius", cone.radius); - ar& make_nvp("halfLength", cone.halfLength); -} - -template -void serialize(Archive& ar, hpp::fcl::Cylinder& cylinder, - const unsigned int /*version*/) { - ar& make_nvp( - "base", boost::serialization::base_object(cylinder)); - ar& make_nvp("radius", cylinder.radius); - ar& make_nvp("halfLength", cylinder.halfLength); -} - -template -void serialize(Archive& ar, hpp::fcl::Halfspace& half_space, - const unsigned int /*version*/) { - ar& make_nvp("base", boost::serialization::base_object( - half_space)); - ar& make_nvp("n", half_space.n); - ar& make_nvp("d", half_space.d); -} - -template -void serialize(Archive& ar, hpp::fcl::Plane& plane, - const unsigned int /*version*/) { - ar& make_nvp("base", - boost::serialization::base_object(plane)); - ar& make_nvp("n", plane.n); - ar& make_nvp("d", plane.d); -} - -} // namespace serialization -} // namespace boost - -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::ShapeBase) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::CollisionGeometry) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::TriangleP) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Box) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Sphere) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Ellipsoid) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Capsule) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Cone) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Cylinder) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Halfspace) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Plane) - -#endif // ifndef HPP_FCL_SERIALIZATION_GEOMETRIC_SHAPES_H +#include +#include diff --git a/include/hpp/fcl/serialization/hfield.h b/include/hpp/fcl/serialization/hfield.h index 4e456a78a..e0e862a73 100644 --- a/include/hpp/fcl/serialization/hfield.h +++ b/include/hpp/fcl/serialization/hfield.h @@ -1,81 +1,2 @@ -// -// Copyright (c) 2021-2024 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_HFIELD_H -#define HPP_FCL_SERIALIZATION_HFIELD_H - -#include "hpp/fcl/hfield.h" - -#include "hpp/fcl/serialization/fwd.h" -#include "hpp/fcl/serialization/OBBRSS.h" - -namespace boost { -namespace serialization { - -template -void serialize(Archive &ar, hpp::fcl::HFNodeBase &node, - const unsigned int /*version*/) { - ar &make_nvp("first_child", node.first_child); - ar &make_nvp("x_id", node.x_id); - ar &make_nvp("x_size", node.x_size); - ar &make_nvp("y_id", node.y_id); - ar &make_nvp("y_size", node.y_size); - ar &make_nvp("max_height", node.max_height); - ar &make_nvp("contact_active_faces", node.contact_active_faces); -} - -template -void serialize(Archive &ar, hpp::fcl::HFNode &node, - const unsigned int /*version*/) { - ar &make_nvp("base", - boost::serialization::base_object(node)); - ar &make_nvp("bv", node.bv); -} - -namespace internal { -template -struct HeightFieldAccessor : hpp::fcl::HeightField { - typedef hpp::fcl::HeightField Base; - using Base::bvs; - using Base::heights; - using Base::max_height; - using Base::min_height; - using Base::num_bvs; - using Base::x_dim; - using Base::x_grid; - using Base::y_dim; - using Base::y_grid; -}; -} // namespace internal - -template -void serialize(Archive &ar, hpp::fcl::HeightField &hf_model, - const unsigned int /*version*/) { - ar &make_nvp( - "base", - boost::serialization::base_object(hf_model)); - - typedef internal::HeightFieldAccessor Accessor; - Accessor &access = reinterpret_cast(hf_model); - - ar &make_nvp("x_dim", access.x_dim); - ar &make_nvp("y_dim", access.y_dim); - ar &make_nvp("heights", access.heights); - ar &make_nvp("min_height", access.min_height); - ar &make_nvp("max_height", access.max_height); - ar &make_nvp("x_grid", access.x_grid); - ar &make_nvp("y_grid", access.y_grid); - - ar &make_nvp("bvs", access.bvs); - ar &make_nvp("num_bvs", access.num_bvs); -} -} // namespace serialization -} // namespace boost - -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::HeightField<::hpp::fcl::AABB>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::HeightField<::hpp::fcl::OBB>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT( - ::hpp::fcl::HeightField<::hpp::fcl::OBBRSS>) - -#endif // ifndef HPP_FCL_SERIALIZATION_HFIELD_H +#include +#include diff --git a/include/hpp/fcl/serialization/kDOP.h b/include/hpp/fcl/serialization/kDOP.h index 36edda0d3..fe4acc157 100644 --- a/include/hpp/fcl/serialization/kDOP.h +++ b/include/hpp/fcl/serialization/kDOP.h @@ -1,34 +1,2 @@ -// -// Copyright (c) 2024 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_kDOP_H -#define HPP_FCL_SERIALIZATION_kDOP_H - -#include "hpp/fcl/BV/kDOP.h" - -#include "hpp/fcl/serialization/fwd.h" - -namespace boost { -namespace serialization { - -namespace internal { -template -struct KDOPAccessor : hpp::fcl::KDOP { - typedef hpp::fcl::KDOP Base; - using Base::dist_; -}; -} // namespace internal - -template -void serialize(Archive& ar, hpp::fcl::KDOP& bv_, - const unsigned int /*version*/) { - typedef internal::KDOPAccessor Accessor; - Accessor& access = reinterpret_cast(bv_); - ar& make_nvp("distances", make_array(access.dist_.data(), N)); -} - -} // namespace serialization -} // namespace boost - -#endif // HPP_FCL_SERIALIZATION_kDOP_H +#include +#include diff --git a/include/hpp/fcl/serialization/kIOS.h b/include/hpp/fcl/serialization/kIOS.h index 8ed8d8725..41fe0d7f6 100644 --- a/include/hpp/fcl/serialization/kIOS.h +++ b/include/hpp/fcl/serialization/kIOS.h @@ -1,58 +1,2 @@ -// -// Copyright (c) 2024 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_kIOS_H -#define HPP_FCL_SERIALIZATION_kIOS_H - -#include "hpp/fcl/BV/kIOS.h" - -#include "hpp/fcl/serialization/OBB.h" -#include "hpp/fcl/serialization/fwd.h" - -namespace boost { -namespace serialization { - -template -void serialize(Archive& ar, hpp::fcl::kIOS& bv, const unsigned int version) { - split_free(ar, bv, version); -} - -template -void save(Archive& ar, const hpp::fcl::kIOS& bv, - const unsigned int /*version*/) { - // Number of spheres in kIOS is never larger than kIOS::kios_max_num_spheres - ar& make_nvp("num_spheres", bv.num_spheres); - - std::array centers{}; - std::array radii; - for (std::size_t i = 0; i < hpp::fcl::kIOS::max_num_spheres; ++i) { - centers[i] = bv.spheres[i].o; - radii[i] = bv.spheres[i].r; - } - ar& make_nvp("centers", make_array(centers.data(), centers.size())); - ar& make_nvp("radii", make_array(radii.data(), radii.size())); - - ar& make_nvp("obb", bv.obb); -} - -template -void load(Archive& ar, hpp::fcl::kIOS& bv, const unsigned int /*version*/) { - ar >> make_nvp("num_spheres", bv.num_spheres); - - std::array centers; - std::array radii; - ar >> make_nvp("centers", make_array(centers.data(), centers.size())); - ar >> make_nvp("radii", make_array(radii.data(), radii.size())); - for (std::size_t i = 0; i < hpp::fcl::kIOS::max_num_spheres; ++i) { - bv.spheres[i].o = centers[i]; - bv.spheres[i].r = radii[i]; - } - - ar >> make_nvp("obb", bv.obb); -} - -} // namespace serialization -} // namespace boost - -#endif // HPP_FCL_SERIALIZATION_kIOS_H +#include +#include diff --git a/include/hpp/fcl/serialization/memory.h b/include/hpp/fcl/serialization/memory.h index e1e3d320f..b03c4026a 100644 --- a/include/hpp/fcl/serialization/memory.h +++ b/include/hpp/fcl/serialization/memory.h @@ -1,32 +1,2 @@ -// -// Copyright (c) 2021 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_MEMORY_H -#define HPP_FCL_SERIALIZATION_MEMORY_H - -namespace hpp { -namespace fcl { - -namespace internal { -template -struct memory_footprint_evaluator { - static size_t run(const T &) { return sizeof(T); } -}; -} // namespace internal - -/// \brief Returns the memory footpring of the input object. -/// For POD objects, this function returns the result of sizeof(T) -/// -/// \param[in] object whose memory footprint needs to be evaluated. -/// -/// \return the memory footprint of the input object. -template -size_t computeMemoryFootprint(const T &object) { - return internal::memory_footprint_evaluator::run(object); -} - -} // namespace fcl -} // namespace hpp - -#endif // ifndef HPP_FCL_SERIALIZATION_MEMORY_H +#include +#include diff --git a/include/hpp/fcl/serialization/octree.h b/include/hpp/fcl/serialization/octree.h index 8ea430022..6d2694aa4 100644 --- a/include/hpp/fcl/serialization/octree.h +++ b/include/hpp/fcl/serialization/octree.h @@ -1,103 +1,2 @@ -// -// Copyright (c) 2023-2024 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_OCTREE_H -#define HPP_FCL_SERIALIZATION_OCTREE_H - -#include -#include - -#include - -#include "hpp/fcl/octree.h" -#include "hpp/fcl/serialization/fwd.h" - -namespace boost { -namespace serialization { - -namespace internal { -struct OcTreeAccessor : hpp::fcl::OcTree { - typedef hpp::fcl::OcTree Base; - using Base::default_occupancy; - using Base::free_threshold; - using Base::occupancy_threshold; - using Base::tree; -}; -} // namespace internal - -template -void save_construct_data(Archive &ar, const hpp::fcl::OcTree *octree_ptr, - const unsigned int /*version*/) { - const double resolution = octree_ptr->getResolution(); - ar << make_nvp("resolution", resolution); -} - -template -void save(Archive &ar, const hpp::fcl::OcTree &octree, - const unsigned int /*version*/) { - typedef internal::OcTreeAccessor Accessor; - const Accessor &access = reinterpret_cast(octree); - - std::ostringstream stream; - access.tree->write(stream); - const std::string stream_str = stream.str(); - auto size = stream_str.size(); - // We can't directly serialize stream_str because it contains binary data. - // This create a bug on Windows with text_archive - ar << make_nvp("tree_data_size", size); - ar << make_nvp("tree_data", - make_array(stream_str.c_str(), stream_str.size())); - - ar << make_nvp("base", base_object(octree)); - ar << make_nvp("default_occupancy", access.default_occupancy); - ar << make_nvp("occupancy_threshold", access.occupancy_threshold); - ar << make_nvp("free_threshold", access.free_threshold); -} - -template -void load_construct_data(Archive &ar, hpp::fcl::OcTree *octree_ptr, - const unsigned int /*version*/) { - double resolution; - ar >> make_nvp("resolution", resolution); - new (octree_ptr) hpp::fcl::OcTree(resolution); -} - -template -void load(Archive &ar, hpp::fcl::OcTree &octree, - const unsigned int /*version*/) { - typedef internal::OcTreeAccessor Accessor; - Accessor &access = reinterpret_cast(octree); - - std::size_t tree_data_size; - ar >> make_nvp("tree_data_size", tree_data_size); - - std::string stream_str; - stream_str.resize(tree_data_size); - /// TODO use stream_str.data in C++17 - assert(tree_data_size > 0 && "tree_data_size should be greater than 0"); - ar >> make_nvp("tree_data", make_array(&stream_str[0], tree_data_size)); - std::istringstream stream(stream_str); - - octomap::AbstractOcTree *new_tree = octomap::AbstractOcTree::read(stream); - access.tree = std::shared_ptr( - dynamic_cast(new_tree)); - - ar >> make_nvp("base", base_object(octree)); - ar >> make_nvp("default_occupancy", access.default_occupancy); - ar >> make_nvp("occupancy_threshold", access.occupancy_threshold); - ar >> make_nvp("free_threshold", access.free_threshold); -} - -template -void serialize(Archive &ar, hpp::fcl::OcTree &octree, - const unsigned int version) { - split_free(ar, octree, version); -} - -} // namespace serialization -} // namespace boost - -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::OcTree) - -#endif // ifndef HPP_FCL_SERIALIZATION_OCTREE_H +#include +#include diff --git a/include/hpp/fcl/serialization/quadrilateral.h b/include/hpp/fcl/serialization/quadrilateral.h index 3694607ea..641f8161d 100644 --- a/include/hpp/fcl/serialization/quadrilateral.h +++ b/include/hpp/fcl/serialization/quadrilateral.h @@ -1,26 +1,2 @@ -// -// Copyright (c) 2022 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_QUADRILATERAL_H -#define HPP_FCL_SERIALIZATION_QUADRILATERAL_H - -#include "hpp/fcl/data_types.h" -#include "hpp/fcl/serialization/fwd.h" - -namespace boost { -namespace serialization { - -template -void serialize(Archive &ar, hpp::fcl::Quadrilateral &quadrilateral, - const unsigned int /*version*/) { - ar &make_nvp("p0", quadrilateral[0]); - ar &make_nvp("p1", quadrilateral[1]); - ar &make_nvp("p2", quadrilateral[2]); - ar &make_nvp("p3", quadrilateral[3]); -} - -} // namespace serialization -} // namespace boost - -#endif // ifndef HPP_FCL_SERIALIZATION_QUADRILATERAL_H +#include +#include diff --git a/include/hpp/fcl/serialization/serializer.h b/include/hpp/fcl/serialization/serializer.h index d0d33cd83..6b194a18e 100644 --- a/include/hpp/fcl/serialization/serializer.h +++ b/include/hpp/fcl/serialization/serializer.h @@ -1,94 +1,2 @@ -// -// Copyright (c) 2024 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_SERIALIZER_H -#define HPP_FCL_SERIALIZATION_SERIALIZER_H - -#include "hpp/fcl/serialization/archive.h" - -namespace hpp { -namespace fcl { -namespace serialization { - -struct Serializer { - /// \brief Loads an object from a text file. - template - static void loadFromText(T& object, const std::string& filename) { - ::hpp::fcl::serialization::loadFromText(object, filename); - } - - /// \brief Saves an object as a text file. - template - static void saveToText(const T& object, const std::string& filename) { - ::hpp::fcl::serialization::saveToText(object, filename); - } - - /// \brief Loads an object from a stream string. - template - static void loadFromStringStream(T& object, std::istringstream& is) { - ::hpp::fcl::serialization::loadFromStringStream(object, is); - } - - /// \brief Saves an object to a string stream. - template - static void saveToStringStream(const T& object, std::stringstream& ss) { - ::hpp::fcl::serialization::saveToStringStream(object, ss); - } - - /// \brief Loads an object from a string. - template - static void loadFromString(T& object, const std::string& str) { - ::hpp::fcl::serialization::loadFromString(object, str); - } - - /// \brief Saves a Derived object to a string. - template - static std::string saveToString(const T& object) { - return ::hpp::fcl::serialization::saveToString(object); - } - - /// \brief Loads an object from an XML file. - template - static void loadFromXML(T& object, const std::string& filename, - const std::string& tag_name) { - ::hpp::fcl::serialization::loadFromXML(object, filename, tag_name); - } - - /// \brief Saves an object as an XML file. - template - static void saveToXML(const T& object, const std::string& filename, - const std::string& tag_name) { - ::hpp::fcl::serialization::saveToXML(object, filename, tag_name); - } - - /// \brief Loads a Derived object from an binary file. - template - static void loadFromBinary(T& object, const std::string& filename) { - ::hpp::fcl::serialization::loadFromBinary(object, filename); - } - - /// \brief Saves a Derived object as an binary file. - template - static void saveToBinary(const T& object, const std::string& filename) { - ::hpp::fcl::serialization::saveToBinary(object, filename); - } - - /// \brief Loads an object from a binary buffer. - template - static void loadFromBuffer(T& object, boost::asio::streambuf& container) { - ::hpp::fcl::serialization::loadFromBuffer(object, container); - } - - /// \brief Saves an object as a binary buffer. - template - static void saveToBuffer(const T& object, boost::asio::streambuf& container) { - ::hpp::fcl::serialization::saveToBuffer(object, container); - } -}; - -} // namespace serialization -} // namespace fcl -} // namespace hpp - -#endif // ifndef HPP_FCL_SERIALIZATION_SERIALIZER_H +#include +#include diff --git a/include/hpp/fcl/serialization/transform.h b/include/hpp/fcl/serialization/transform.h index 4c251c447..a72222f79 100644 --- a/include/hpp/fcl/serialization/transform.h +++ b/include/hpp/fcl/serialization/transform.h @@ -1,24 +1,2 @@ -// -// Copyright (c) 2024 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_TRANSFORM_H -#define HPP_FCL_SERIALIZATION_TRANSFORM_H - -#include "hpp/fcl/math/transform.h" -#include "hpp/fcl/serialization/fwd.h" - -namespace boost { -namespace serialization { - -template -void serialize(Archive& ar, hpp::fcl::Transform3f& tf, - const unsigned int /*version*/) { - ar& make_nvp("R", tf.rotation()); - ar& make_nvp("T", tf.translation()); -} - -} // namespace serialization -} // namespace boost - -#endif // HPP_FCL_SERIALIZATION_TRANSFORM_H +#include +#include diff --git a/include/hpp/fcl/serialization/triangle.h b/include/hpp/fcl/serialization/triangle.h index 44c532e72..5c2b23f13 100644 --- a/include/hpp/fcl/serialization/triangle.h +++ b/include/hpp/fcl/serialization/triangle.h @@ -1,25 +1,2 @@ -// -// Copyright (c) 2021-2022 INRIA -// - -#ifndef HPP_FCL_SERIALIZATION_TRIANGLE_H -#define HPP_FCL_SERIALIZATION_TRIANGLE_H - -#include "hpp/fcl/data_types.h" -#include "hpp/fcl/serialization/fwd.h" - -namespace boost { -namespace serialization { - -template -void serialize(Archive &ar, hpp::fcl::Triangle &triangle, - const unsigned int /*version*/) { - ar &make_nvp("p0", triangle[0]); - ar &make_nvp("p1", triangle[1]); - ar &make_nvp("p2", triangle[2]); -} - -} // namespace serialization -} // namespace boost - -#endif // ifndef HPP_FCL_SERIALIZATION_TRIANGLE_H +#include +#include diff --git a/include/hpp/fcl/shape/convex.h b/include/hpp/fcl/shape/convex.h index a69d5c657..0d5dd44ab 100644 --- a/include/hpp/fcl/shape/convex.h +++ b/include/hpp/fcl/shape/convex.h @@ -1,115 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_SHAPE_CONVEX_H -#define HPP_FCL_SHAPE_CONVEX_H - -#include "hpp/fcl/shape/geometric_shapes.h" - -namespace hpp { -namespace fcl { - -/// @brief Convex polytope -/// @tparam PolygonT the polygon class. It must have method \c size() and -/// \c operator[](int i) -template -class Convex : public ConvexBase { - public: - /// @brief Construct an uninitialized convex object - Convex() : ConvexBase(), num_polygons(0) {} - - /// @brief Constructing a convex, providing normal and offset of each polytype - /// surface, and the points and shape topology information \param ownStorage - /// whether this class owns the pointers of points and - /// polygons. If owned, they are deleted upon destruction. - /// \param points_ list of 3D points - /// \param num_points_ number of 3D points - /// \param polygons_ \copydoc Convex::polygons - /// \param num_polygons_ the number of polygons. - /// \note num_polygons_ is not the allocated size of polygons_. - Convex(std::shared_ptr> points_, unsigned int num_points_, - std::shared_ptr> polygons_, - unsigned int num_polygons_); - - /// @brief Copy constructor - /// Only the list of neighbors is copied. - Convex(const Convex& other); - - ~Convex(); - - /// based on http://number-none.com/blow/inertia/bb_inertia.doc - Matrix3f computeMomentofInertia() const; - - Vec3f computeCOM() const; - - FCL_REAL computeVolume() const; - - /// - /// @brief Set the current Convex from a list of points and polygons. - /// - /// \param ownStorage whether this class owns the pointers of points and - /// polygons. If owned, they are deleted upon destruction. - /// \param points list of 3D points - /// \param num_points number of 3D points - /// \param polygons \copydoc Convex::polygons - /// \param num_polygons the number of polygons. - /// \note num_polygons is not the allocated size of polygons. - /// - void set(std::shared_ptr> points, unsigned int num_points, - std::shared_ptr> polygons, - unsigned int num_polygons); - - /// @brief Clone (deep copy) - virtual Convex* clone() const; - - /// @brief An array of PolygonT object. - /// PolygonT should contains a list of vertices for each polygon, - /// in counter clockwise order. - std::shared_ptr> polygons; - unsigned int num_polygons; - - protected: - void fillNeighbors(); -}; - -} // namespace fcl - -} // namespace hpp - -#include "hpp/fcl/shape/details/convex.hxx" - -#endif +#include +#include diff --git a/include/hpp/fcl/shape/details/convex.hxx b/include/hpp/fcl/shape/details/convex.hxx index 373921e6f..d7b78f4d7 100644 --- a/include/hpp/fcl/shape/details/convex.hxx +++ b/include/hpp/fcl/shape/details/convex.hxx @@ -1,286 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, CNRS - LAAS - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Joseph Mirabel */ - -#ifndef HPP_FCL_SHAPE_CONVEX_HXX -#define HPP_FCL_SHAPE_CONVEX_HXX - -#include -#include -#include - -namespace hpp { -namespace fcl { - -template -Convex::Convex(std::shared_ptr> points_, - unsigned int num_points_, - std::shared_ptr> polygons_, - unsigned int num_polygons_) - : ConvexBase(), polygons(polygons_), num_polygons(num_polygons_) { - this->initialize(points_, num_points_); - this->fillNeighbors(); - this->buildSupportWarmStart(); -} - -template -Convex::Convex(const Convex& other) - : ConvexBase(other), num_polygons(other.num_polygons) { - if (other.polygons.get()) { - polygons.reset(new std::vector(*(other.polygons))); - } else - polygons.reset(); -} - -template -Convex::~Convex() {} - -template -void Convex::set(std::shared_ptr> points_, - unsigned int num_points_, - std::shared_ptr> polygons_, - unsigned int num_polygons_) { - ConvexBase::set(points_, num_points_); - - this->num_polygons = num_polygons_; - this->polygons = polygons_; - - this->fillNeighbors(); - this->buildSupportWarmStart(); -} - -template -Convex* Convex::clone() const { - return new Convex(*this); -} - -template -Matrix3f Convex::computeMomentofInertia() const { - typedef typename PolygonT::size_type size_type; - typedef typename PolygonT::index_type index_type; - - Matrix3f C = Matrix3f::Zero(); - - Matrix3f C_canonical; - C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0, 1 / 120.0, - 1 / 120.0, 1 / 120.0, 1 / 60.0; - - if (!(points.get())) { - std::cerr - << "Error in `Convex::computeMomentofInertia`! Convex has no vertices." - << std::endl; - return C; - } - const std::vector& points_ = *points; - if (!(polygons.get())) { - std::cerr - << "Error in `Convex::computeMomentofInertia`! Convex has no polygons." - << std::endl; - return C; - } - const std::vector& polygons_ = *polygons; - for (unsigned int i = 0; i < num_polygons; ++i) { - const PolygonT& polygon = polygons_[i]; - - // compute the center of the polygon - Vec3f plane_center(0, 0, 0); - for (size_type j = 0; j < polygon.size(); ++j) - plane_center += points_[polygon[(index_type)j]]; - plane_center /= polygon.size(); - - // compute the volume of tetrahedron making by neighboring two points, the - // plane center and the reference point (zero) of the convex shape - const Vec3f& v3 = plane_center; - for (size_type j = 0; j < polygon.size(); ++j) { - index_type e_first = polygon[static_cast(j)]; - index_type e_second = - polygon[static_cast((j + 1) % polygon.size())]; - const Vec3f& v1 = points_[e_first]; - const Vec3f& v2 = points_[e_second]; - Matrix3f A; - A << v1.transpose(), v2.transpose(), - v3.transpose(); // this is A' in the original document - C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3); - } - } - - return C.trace() * Matrix3f::Identity() - C; -} - -template -Vec3f Convex::computeCOM() const { - typedef typename PolygonT::size_type size_type; - typedef typename PolygonT::index_type index_type; - - Vec3f com(0, 0, 0); - FCL_REAL vol = 0; - if (!(points.get())) { - std::cerr << "Error in `Convex::computeCOM`! Convex has no vertices." - << std::endl; - return com; - } - const std::vector& points_ = *points; - if (!(polygons.get())) { - std::cerr << "Error in `Convex::computeCOM`! Convex has no polygons." - << std::endl; - return com; - } - const std::vector& polygons_ = *polygons; - for (unsigned int i = 0; i < num_polygons; ++i) { - const PolygonT& polygon = polygons_[i]; - // compute the center of the polygon - Vec3f plane_center(0, 0, 0); - for (size_type j = 0; j < polygon.size(); ++j) - plane_center += points_[polygon[(index_type)j]]; - plane_center /= polygon.size(); - - // compute the volume of tetrahedron making by neighboring two points, the - // plane center and the reference point (zero) of the convex shape - const Vec3f& v3 = plane_center; - for (size_type j = 0; j < polygon.size(); ++j) { - index_type e_first = polygon[static_cast(j)]; - index_type e_second = - polygon[static_cast((j + 1) % polygon.size())]; - const Vec3f& v1 = points_[e_first]; - const Vec3f& v2 = points_[e_second]; - FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3); - vol += d_six_vol; - com += (points_[e_first] + points_[e_second] + plane_center) * d_six_vol; - } - } - - return com / (vol * 4); // here we choose zero as the reference -} - -template -FCL_REAL Convex::computeVolume() const { - typedef typename PolygonT::size_type size_type; - typedef typename PolygonT::index_type index_type; - - FCL_REAL vol = 0; - if (!(points.get())) { - std::cerr << "Error in `Convex::computeVolume`! Convex has no vertices." - << std::endl; - return vol; - } - const std::vector& points_ = *points; - if (!(polygons.get())) { - std::cerr << "Error in `Convex::computeVolume`! Convex has no polygons." - << std::endl; - return vol; - } - const std::vector& polygons_ = *polygons; - for (unsigned int i = 0; i < num_polygons; ++i) { - const PolygonT& polygon = polygons_[i]; - - // compute the center of the polygon - Vec3f plane_center(0, 0, 0); - for (size_type j = 0; j < polygon.size(); ++j) - plane_center += points_[polygon[(index_type)j]]; - plane_center /= polygon.size(); - - // compute the volume of tetrahedron making by neighboring two points, the - // plane center and the reference point (zero point) of the convex shape - const Vec3f& v3 = plane_center; - for (size_type j = 0; j < polygon.size(); ++j) { - index_type e_first = polygon[static_cast(j)]; - index_type e_second = - polygon[static_cast((j + 1) % polygon.size())]; - const Vec3f& v1 = points_[e_first]; - const Vec3f& v2 = points_[e_second]; - FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3); - vol += d_six_vol; - } - } - - return vol / 6; -} - -template -void Convex::fillNeighbors() { - neighbors.reset(new std::vector(num_points)); - - typedef typename PolygonT::size_type size_type; - typedef typename PolygonT::index_type index_type; - std::vector> nneighbors(num_points); - unsigned int c_nneighbors = 0; - - if (!(polygons.get())) { - std::cerr << "Error in `Convex::fillNeighbors`! Convex has no polygons." - << std::endl; - } - const std::vector& polygons_ = *polygons; - for (unsigned int l = 0; l < num_polygons; ++l) { - const PolygonT& polygon = polygons_[l]; - const size_type n = polygon.size(); - - for (size_type j = 0; j < polygon.size(); ++j) { - size_type i = (j == 0) ? n - 1 : j - 1; - size_type k = (j == n - 1) ? 0 : j + 1; - index_type pi = polygon[(index_type)i], pj = polygon[(index_type)j], - pk = polygon[(index_type)k]; - // Update neighbors of pj; - if (nneighbors[pj].count(pi) == 0) { - c_nneighbors++; - nneighbors[pj].insert(pi); - } - if (nneighbors[pj].count(pk) == 0) { - c_nneighbors++; - nneighbors[pj].insert(pk); - } - } - } - - nneighbors_.reset(new std::vector(c_nneighbors)); - - unsigned int* p_nneighbors = nneighbors_->data(); - std::vector& neighbors_ = *neighbors; - for (unsigned int i = 0; i < num_points; ++i) { - Neighbors& n = neighbors_[i]; - if (nneighbors[i].size() >= (std::numeric_limits::max)()) - HPP_FCL_THROW_PRETTY("Too many neighbors.", std::logic_error); - n.count_ = (unsigned char)nneighbors[i].size(); - n.n_ = p_nneighbors; - p_nneighbors = - std::copy(nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors); - } - assert(p_nneighbors == nneighbors_->data() + c_nneighbors); -} - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h b/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h index dec758e54..66bb3b7e6 100644 --- a/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h +++ b/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h @@ -1,357 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef GEOMETRIC_SHAPE_TO_BVH_MODEL_H -#define GEOMETRIC_SHAPE_TO_BVH_MODEL_H - -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// @brief Generate BVH model from box -template -void generateBVHModel(BVHModel& model, const Box& shape, - const Transform3f& pose) { - FCL_REAL a = shape.halfSide[0]; - FCL_REAL b = shape.halfSide[1]; - FCL_REAL c = shape.halfSide[2]; - std::vector points(8); - std::vector tri_indices(12); - points[0] = Vec3f(a, -b, c); - points[1] = Vec3f(a, b, c); - points[2] = Vec3f(-a, b, c); - points[3] = Vec3f(-a, -b, c); - points[4] = Vec3f(a, -b, -c); - points[5] = Vec3f(a, b, -c); - points[6] = Vec3f(-a, b, -c); - points[7] = Vec3f(-a, -b, -c); - - tri_indices[0].set(0, 4, 1); - tri_indices[1].set(1, 4, 5); - tri_indices[2].set(2, 6, 3); - tri_indices[3].set(3, 6, 7); - tri_indices[4].set(3, 0, 2); - tri_indices[5].set(2, 0, 1); - tri_indices[6].set(6, 5, 7); - tri_indices[7].set(7, 5, 4); - tri_indices[8].set(1, 5, 2); - tri_indices[9].set(2, 5, 6); - tri_indices[10].set(3, 7, 0); - tri_indices[11].set(0, 7, 4); - - for (unsigned int i = 0; i < points.size(); ++i) { - points[i] = pose.transform(points[i]).eval(); - } - - model.beginModel(); - model.addSubModel(points, tri_indices); - model.endModel(); - model.computeLocalAABB(); -} - -/// @brief Generate BVH model from sphere, given the number of segments along -/// longitude and number of rings along latitude. -template -void generateBVHModel(BVHModel& model, const Sphere& shape, - const Transform3f& pose, unsigned int seg, - unsigned int ring) { - std::vector points; - std::vector tri_indices; - - FCL_REAL r = shape.radius; - FCL_REAL phi, phid; - const FCL_REAL pi = boost::math::constants::pi(); - phid = pi * 2 / seg; - phi = 0; - - FCL_REAL theta, thetad; - thetad = pi / (ring + 1); - theta = 0; - - for (unsigned int i = 0; i < ring; ++i) { - FCL_REAL theta_ = theta + thetad * (i + 1); - for (unsigned int j = 0; j < seg; ++j) { - points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid), - r * sin(theta_) * sin(phi + j * phid), - r * cos(theta_))); - } - } - points.push_back(Vec3f(0, 0, r)); - points.push_back(Vec3f(0, 0, -r)); - - for (unsigned int i = 0; i < ring - 1; ++i) { - for (unsigned int j = 0; j < seg; ++j) { - unsigned int a, b, c, d; - a = i * seg + j; - b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1); - c = (i + 1) * seg + j; - d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1); - tri_indices.push_back(Triangle(a, c, b)); - tri_indices.push_back(Triangle(b, c, d)); - } - } - - for (unsigned int j = 0; j < seg; ++j) { - unsigned int a, b; - a = j; - b = (j == seg - 1) ? 0 : (j + 1); - tri_indices.push_back(Triangle(ring * seg, a, b)); - - a = (ring - 1) * seg + j; - b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1); - tri_indices.push_back(Triangle(a, ring * seg + 1, b)); - } - - for (unsigned int i = 0; i < points.size(); ++i) { - points[i] = pose.transform(points[i]); - } - - model.beginModel(); - model.addSubModel(points, tri_indices); - model.endModel(); - model.computeLocalAABB(); -} - -/// @brief Generate BVH model from sphere -/// The difference between generateBVHModel is that it gives the number of -/// triangles faces N for a sphere with unit radius. For sphere of radius r, -/// then the number of triangles is r * r * N so that the area represented by a -/// single triangle is approximately the same.s -template -void generateBVHModel(BVHModel& model, const Sphere& shape, - const Transform3f& pose, - unsigned int n_faces_for_unit_sphere) { - FCL_REAL r = shape.radius; - FCL_REAL n_low_bound = - std::sqrt((FCL_REAL)n_faces_for_unit_sphere / FCL_REAL(2.)) * r * r; - unsigned int ring = (unsigned int)ceil(n_low_bound); - unsigned int seg = (unsigned int)ceil(n_low_bound); - - generateBVHModel(model, shape, pose, seg, ring); -} - -/// @brief Generate BVH model from cylinder, given the number of segments along -/// circle and the number of segments along axis. -template -void generateBVHModel(BVHModel& model, const Cylinder& shape, - const Transform3f& pose, unsigned int tot, - unsigned int h_num) { - std::vector points; - std::vector tri_indices; - - FCL_REAL r = shape.radius; - FCL_REAL h = shape.halfLength; - FCL_REAL phi, phid; - const FCL_REAL pi = boost::math::constants::pi(); - phid = pi * 2 / tot; - phi = 0; - - FCL_REAL hd = 2 * h / h_num; - - for (unsigned int i = 0; i < tot; ++i) - points.push_back( - Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h)); - - for (unsigned int i = 0; i < h_num - 1; ++i) { - for (unsigned int j = 0; j < tot; ++j) { - points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), - h - (i + 1) * hd)); - } - } - - for (unsigned int i = 0; i < tot; ++i) - points.push_back( - Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h)); - - points.push_back(Vec3f(0, 0, h)); - points.push_back(Vec3f(0, 0, -h)); - - for (unsigned int i = 0; i < tot; ++i) { - Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1))); - tri_indices.push_back(tmp); - } - - for (unsigned int i = 0; i < tot; ++i) { - Triangle tmp((h_num + 1) * tot + 1, - h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i); - tri_indices.push_back(tmp); - } - - for (unsigned int i = 0; i < h_num; ++i) { - for (unsigned int j = 0; j < tot; ++j) { - unsigned int a, b, c, d; - a = j; - b = (j == tot - 1) ? 0 : (j + 1); - c = j + tot; - d = (j == tot - 1) ? tot : (j + 1 + tot); - - unsigned int start = i * tot; - tri_indices.push_back(Triangle(start + b, start + a, start + c)); - tri_indices.push_back(Triangle(start + b, start + c, start + d)); - } - } - - for (unsigned int i = 0; i < points.size(); ++i) { - points[i] = pose.transform(points[i]); - } - - model.beginModel(); - model.addSubModel(points, tri_indices); - model.endModel(); - model.computeLocalAABB(); -} - -/// @brief Generate BVH model from cylinder -/// Difference from generateBVHModel: is that it gives the circle split number -/// tot for a cylinder with unit radius. For cylinder with larger radius, the -/// number of circle split number is r * tot. -template -void generateBVHModel(BVHModel& model, const Cylinder& shape, - const Transform3f& pose, - unsigned int tot_for_unit_cylinder) { - FCL_REAL r = shape.radius; - FCL_REAL h = 2 * shape.halfLength; - - const FCL_REAL pi = boost::math::constants::pi(); - unsigned int tot = (unsigned int)(tot_for_unit_cylinder * r); - FCL_REAL phid = pi * 2 / tot; - - FCL_REAL circle_edge = phid * r; - unsigned int h_num = (unsigned int)ceil(h / circle_edge); - - generateBVHModel(model, shape, pose, tot, h_num); -} - -/// @brief Generate BVH model from cone, given the number of segments along -/// circle and the number of segments along axis. -template -void generateBVHModel(BVHModel& model, const Cone& shape, - const Transform3f& pose, unsigned int tot, - unsigned int h_num) { - std::vector points; - std::vector tri_indices; - - FCL_REAL r = shape.radius; - FCL_REAL h = shape.halfLength; - - FCL_REAL phi, phid; - const FCL_REAL pi = boost::math::constants::pi(); - phid = pi * 2 / tot; - phi = 0; - - FCL_REAL hd = 2 * h / h_num; - - for (unsigned int i = 0; i < h_num - 1; ++i) { - FCL_REAL h_i = h - (i + 1) * hd; - FCL_REAL rh = r * (0.5 - h_i / h / 2); - for (unsigned int j = 0; j < tot; ++j) { - points.push_back( - Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i)); - } - } - - for (unsigned int i = 0; i < tot; ++i) - points.push_back( - Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h)); - - points.push_back(Vec3f(0, 0, h)); - points.push_back(Vec3f(0, 0, -h)); - - for (unsigned int i = 0; i < tot; ++i) { - Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1)); - tri_indices.push_back(tmp); - } - - for (unsigned int i = 0; i < tot; ++i) { - Triangle tmp(h_num * tot + 1, - (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), - (h_num - 1) * tot + i); - tri_indices.push_back(tmp); - } - - for (unsigned int i = 0; i < h_num - 1; ++i) { - for (unsigned int j = 0; j < tot; ++j) { - unsigned int a, b, c, d; - a = j; - b = (j == tot - 1) ? 0 : (j + 1); - c = j + tot; - d = (j == tot - 1) ? tot : (j + 1 + tot); - - unsigned int start = i * tot; - tri_indices.push_back(Triangle(start + b, start + a, start + c)); - tri_indices.push_back(Triangle(start + b, start + c, start + d)); - } - } - - for (unsigned int i = 0; i < points.size(); ++i) { - points[i] = pose.transform(points[i]); - } - - model.beginModel(); - model.addSubModel(points, tri_indices); - model.endModel(); - model.computeLocalAABB(); -} - -/// @brief Generate BVH model from cone -/// Difference from generateBVHModel: is that it gives the circle split number -/// tot for a cylinder with unit radius. For cone with larger radius, the number -/// of circle split number is r * tot. -template -void generateBVHModel(BVHModel& model, const Cone& shape, - const Transform3f& pose, unsigned int tot_for_unit_cone) { - FCL_REAL r = shape.radius; - FCL_REAL h = 2 * shape.halfLength; - - const FCL_REAL pi = boost::math::constants::pi(); - unsigned int tot = (unsigned int)(tot_for_unit_cone * r); - FCL_REAL phid = pi * 2 / tot; - - FCL_REAL circle_edge = phid * r; - unsigned int h_num = (unsigned int)ceil(h / circle_edge); - - generateBVHModel(model, shape, pose, tot, h_num); -} - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h index fada3e07a..dd8f59a95 100644 --- a/include/hpp/fcl/shape/geometric_shapes.h +++ b/include/hpp/fcl/shape/geometric_shapes.h @@ -1,1054 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_GEOMETRIC_SHAPES_H -#define HPP_FCL_GEOMETRIC_SHAPES_H - -#include -#include - -#include - -#include "hpp/fcl/collision_object.h" -#include "hpp/fcl/data_types.h" - -#ifdef HPP_FCL_HAS_QHULL -namespace orgQhull { -class Qhull; -} -#endif - -namespace hpp { -namespace fcl { - -/// @brief Base class for all basic geometric shapes -class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry { - public: - ShapeBase() {} - - ///  \brief Copy constructor - ShapeBase(const ShapeBase& other) - : CollisionGeometry(other), - m_swept_sphere_radius(other.m_swept_sphere_radius) {} - - ShapeBase& operator=(const ShapeBase& other) = default; - - virtual ~ShapeBase() {}; - - /// @brief Get object type: a geometric shape - OBJECT_TYPE getObjectType() const { return OT_GEOM; } - - /// @brief Set radius of sphere swept around the shape. - /// Must be >= 0. - void setSweptSphereRadius(FCL_REAL radius) { - if (radius < 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius must be positive.", - std::invalid_argument); - } - this->m_swept_sphere_radius = radius; - } - - /// @brief Get radius of sphere swept around the shape. - /// This radius is always >= 0. - FCL_REAL getSweptSphereRadius() const { return this->m_swept_sphere_radius; } - - protected: - /// \brief Radius of the sphere swept around the shape. - /// Default value is 0. - /// Note: this property differs from `inflated` method of certain - /// derived classes (e.g. Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder) - /// in the sense that inflated returns a new shape which can be inflated but - /// also deflated. - /// Also, an inflated shape is not rounded. It simply has a different size. - /// Sweeping a shape with a sphere is a different operation (a Minkowski sum), - /// which rounds the sharp corners of a shape. - /// The swept sphere radius is a property of the shape itself and can be - /// manually updated between collision checks. - FCL_REAL m_swept_sphere_radius{0}; -}; - -/// @defgroup Geometric_Shapes Geometric shapes -/// Classes of different types of geometric shapes. -/// @{ - -/// @brief Triangle stores the points instead of only indices of points -class HPP_FCL_DLLAPI TriangleP : public ShapeBase { - public: - TriangleP() {}; - - TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) - : ShapeBase(), a(a_), b(b_), c(c_) {} - - TriangleP(const TriangleP& other) - : ShapeBase(other), a(other.a), b(other.b), c(other.c) {} - - /// @brief Clone *this into a new TriangleP - virtual TriangleP* clone() const { return new TriangleP(*this); }; - - /// @brief virtual function of compute AABB in local coordinate - void computeLocalAABB(); - - NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; } - - // std::pair inflated(const FCL_REAL value) const { - // if (value == 0) return std::make_pair(new TriangleP(*this), - // Transform3f()); Vec3f AB(b - a), BC(c - b), CA(a - c); AB.normalize(); - // BC.normalize(); - // CA.normalize(); - // - // Vec3f new_a(a + value * Vec3f(-AB + CA).normalized()); - // Vec3f new_b(b + value * Vec3f(-BC + AB).normalized()); - // Vec3f new_c(c + value * Vec3f(-CA + BC).normalized()); - // - // return std::make_pair(new TriangleP(new_a, new_b, new_c), - // Transform3f()); - // } - // - // FCL_REAL minInflationValue() const - // { - // return (std::numeric_limits::max)(); // TODO(jcarpent): - // implement - // } - - Vec3f a, b, c; - - private: - virtual bool isEqual(const CollisionGeometry& _other) const { - const TriangleP* other_ptr = dynamic_cast(&_other); - if (other_ptr == nullptr) return false; - const TriangleP& other = *other_ptr; - - return a == other.a && b == other.b && c == other.c && - getSweptSphereRadius() == other.getSweptSphereRadius(); - } - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/// @brief Center at zero point, axis aligned box -class HPP_FCL_DLLAPI Box : public ShapeBase { - public: - Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) - : ShapeBase(), halfSide(x / 2, y / 2, z / 2) {} - - Box(const Vec3f& side_) : ShapeBase(), halfSide(side_ / 2) {} - - Box(const Box& other) : ShapeBase(other), halfSide(other.halfSide) {} - - Box& operator=(const Box& other) { - if (this == &other) return *this; - - this->halfSide = other.halfSide; - return *this; - } - - /// @brief Clone *this into a new Box - virtual Box* clone() const { return new Box(*this); }; - - /// @brief Default constructor - Box() {} - - /// @brief box side half-length - Vec3f halfSide; - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a box - NODE_TYPE getNodeType() const { return GEOM_BOX; } - - FCL_REAL computeVolume() const { return 8 * halfSide.prod(); } - - Matrix3f computeMomentofInertia() const { - FCL_REAL V = computeVolume(); - Vec3f s(halfSide.cwiseAbs2() * V); - return (Vec3f(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal(); - } - - FCL_REAL minInflationValue() const { return -halfSide.minCoeff(); } - - /// \brief Inflate the box by an amount given by `value`. - /// This value can be positive or negative but must always >= - /// `minInflationValue()`. - /// - /// \param[in] value of the shape inflation. - /// - /// \returns a new inflated box and the related transform to account for the - /// change of shape frame - std::pair inflated(const FCL_REAL value) const { - if (value <= minInflationValue()) - HPP_FCL_THROW_PRETTY("value (" << value << ") " - << "is two small. It should be at least: " - << minInflationValue(), - std::invalid_argument); - return std::make_pair(Box(2 * (halfSide + Vec3f::Constant(value))), - Transform3f()); - } - - private: - virtual bool isEqual(const CollisionGeometry& _other) const { - const Box* other_ptr = dynamic_cast(&_other); - if (other_ptr == nullptr) return false; - const Box& other = *other_ptr; - - return halfSide == other.halfSide && - getSweptSphereRadius() == other.getSweptSphereRadius(); - } - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/// @brief Center at zero point sphere -class HPP_FCL_DLLAPI Sphere : public ShapeBase { - public: - /// @brief Default constructor - Sphere() {} - - explicit Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_) {} - - Sphere(const Sphere& other) : ShapeBase(other), radius(other.radius) {} - - /// @brief Clone *this into a new Sphere - virtual Sphere* clone() const { return new Sphere(*this); }; - - /// @brief Radius of the sphere - FCL_REAL radius; - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a sphere - NODE_TYPE getNodeType() const { return GEOM_SPHERE; } - - Matrix3f computeMomentofInertia() const { - FCL_REAL I = 0.4 * radius * radius * computeVolume(); - return I * Matrix3f::Identity(); - } - - FCL_REAL computeVolume() const { - return 4 * boost::math::constants::pi() * radius * radius * - radius / 3; - } - - FCL_REAL minInflationValue() const { return -radius; } - - /// \brief Inflate the sphere by an amount given by `value`. - /// This value can be positive or negative but must always >= - /// `minInflationValue()`. - /// - /// \param[in] value of the shape inflation. - /// - /// \returns a new inflated sphere and the related transform to account for - /// the change of shape frame - std::pair inflated(const FCL_REAL value) const { - if (value <= minInflationValue()) - HPP_FCL_THROW_PRETTY( - "value (" << value << ") is two small. It should be at least: " - << minInflationValue(), - std::invalid_argument); - return std::make_pair(Sphere(radius + value), Transform3f()); - } - - private: - virtual bool isEqual(const CollisionGeometry& _other) const { - const Sphere* other_ptr = dynamic_cast(&_other); - if (other_ptr == nullptr) return false; - const Sphere& other = *other_ptr; - - return radius == other.radius && - getSweptSphereRadius() == other.getSweptSphereRadius(); - } - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/// @brief Ellipsoid centered at point zero -class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase { - public: - /// @brief Default constructor - Ellipsoid() {} - - Ellipsoid(FCL_REAL rx, FCL_REAL ry, FCL_REAL rz) - : ShapeBase(), radii(rx, ry, rz) {} - - explicit Ellipsoid(const Vec3f& radii) : radii(radii) {} - - Ellipsoid(const Ellipsoid& other) : ShapeBase(other), radii(other.radii) {} - - /// @brief Clone *this into a new Ellipsoid - virtual Ellipsoid* clone() const { return new Ellipsoid(*this); }; - - /// @brief Radii of the Ellipsoid (such that on boundary: x^2/rx^2 + y^2/ry^2 - /// + z^2/rz^2 = 1) - Vec3f radii; - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: an ellipsoid - NODE_TYPE getNodeType() const { return GEOM_ELLIPSOID; } - - Matrix3f computeMomentofInertia() const { - FCL_REAL V = computeVolume(); - FCL_REAL a2 = V * radii[0] * radii[0]; - FCL_REAL b2 = V * radii[1] * radii[1]; - FCL_REAL c2 = V * radii[2] * radii[2]; - return (Matrix3f() << 0.2 * (b2 + c2), 0, 0, 0, 0.2 * (a2 + c2), 0, 0, 0, - 0.2 * (a2 + b2)) - .finished(); - } - - FCL_REAL computeVolume() const { - return 4 * boost::math::constants::pi() * radii[0] * radii[1] * - radii[2] / 3; - } - - FCL_REAL minInflationValue() const { return -radii.minCoeff(); } - - /// \brief Inflate the ellipsoid by an amount given by `value`. - /// This value can be positive or negative but must always >= - /// `minInflationValue()`. - /// - /// \param[in] value of the shape inflation. - /// - /// \returns a new inflated ellipsoid and the related transform to account for - /// the change of shape frame - std::pair inflated(const FCL_REAL value) const { - if (value <= minInflationValue()) - HPP_FCL_THROW_PRETTY( - "value (" << value << ") is two small. It should be at least: " - << minInflationValue(), - std::invalid_argument); - return std::make_pair(Ellipsoid(radii + Vec3f::Constant(value)), - Transform3f()); - } - - private: - virtual bool isEqual(const CollisionGeometry& _other) const { - const Ellipsoid* other_ptr = dynamic_cast(&_other); - if (other_ptr == nullptr) return false; - const Ellipsoid& other = *other_ptr; - - return radii == other.radii && - getSweptSphereRadius() == other.getSweptSphereRadius(); - } - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/// @brief Capsule -/// It is \f$ { x~\in~\mathbb{R}^3, d(x, AB) \leq radius } \f$ -/// where \f$ d(x, AB) \f$ is the distance between the point x and the capsule -/// segment AB, with \f$ A = (0,0,-halfLength), B = (0,0,halfLength) \f$. -class HPP_FCL_DLLAPI Capsule : public ShapeBase { - public: - /// @brief Default constructor - Capsule() {} - - Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) { - halfLength = lz_ / 2; - } - - Capsule(const Capsule& other) - : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {} - - /// @brief Clone *this into a new Capsule - virtual Capsule* clone() const { return new Capsule(*this); }; - - /// @brief Radius of capsule - FCL_REAL radius; - - /// @brief Half Length along z axis - FCL_REAL halfLength; - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a capsule - NODE_TYPE getNodeType() const { return GEOM_CAPSULE; } - - FCL_REAL computeVolume() const { - return boost::math::constants::pi() * radius * radius * - ((halfLength * 2) + radius * 4 / 3.0); - } - - Matrix3f computeMomentofInertia() const { - FCL_REAL v_cyl = radius * radius * (halfLength * 2) * - boost::math::constants::pi(); - FCL_REAL v_sph = radius * radius * radius * - boost::math::constants::pi() * 4 / 3.0; - - FCL_REAL h2 = halfLength * halfLength; - FCL_REAL r2 = radius * radius; - FCL_REAL ix = v_cyl * (h2 / 3. + r2 / 4.) + - v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength); - FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius; - - return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); - } - - FCL_REAL minInflationValue() const { return -radius; } - - /// \brief Inflate the capsule by an amount given by `value`. - /// This value can be positive or negative but must always >= - /// `minInflationValue()`. - /// - /// \param[in] value of the shape inflation. - /// - /// \returns a new inflated capsule and the related transform to account for - /// the change of shape frame - std::pair inflated(const FCL_REAL value) const { - if (value <= minInflationValue()) - HPP_FCL_THROW_PRETTY( - "value (" << value << ") is two small. It should be at least: " - << minInflationValue(), - std::invalid_argument); - return std::make_pair(Capsule(radius + value, 2 * halfLength), - Transform3f()); - } - - private: - virtual bool isEqual(const CollisionGeometry& _other) const { - const Capsule* other_ptr = dynamic_cast(&_other); - if (other_ptr == nullptr) return false; - const Capsule& other = *other_ptr; - - return radius == other.radius && halfLength == other.halfLength && - getSweptSphereRadius() == other.getSweptSphereRadius(); - } - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/// @brief Cone -/// The base of the cone is at \f$ z = - halfLength \f$ and the top is at -/// \f$ z = halfLength \f$. -class HPP_FCL_DLLAPI Cone : public ShapeBase { - public: - /// @brief Default constructor - Cone() {} - - Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) { - halfLength = lz_ / 2; - } - - Cone(const Cone& other) - : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {} - - /// @brief Clone *this into a new Cone - virtual Cone* clone() const { return new Cone(*this); }; - - /// @brief Radius of the cone - FCL_REAL radius; - - /// @brief Half Length along z axis - FCL_REAL halfLength; - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a cone - NODE_TYPE getNodeType() const { return GEOM_CONE; } - - FCL_REAL computeVolume() const { - return boost::math::constants::pi() * radius * radius * - (halfLength * 2) / 3; - } - - Matrix3f computeMomentofInertia() const { - FCL_REAL V = computeVolume(); - FCL_REAL ix = - V * (0.4 * halfLength * halfLength + 3 * radius * radius / 20); - FCL_REAL iz = 0.3 * V * radius * radius; - - return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); - } - - Vec3f computeCOM() const { return Vec3f(0, 0, -0.5 * halfLength); } - - FCL_REAL minInflationValue() const { return -(std::min)(radius, halfLength); } - - /// \brief Inflate the cone by an amount given by `value`. - /// This value can be positive or negative but must always >= - /// `minInflationValue()`. - /// - /// \param[in] value of the shape inflation. - /// - /// \returns a new inflated cone and the related transform to account for the - /// change of shape frame - std::pair inflated(const FCL_REAL value) const { - if (value <= minInflationValue()) - HPP_FCL_THROW_PRETTY( - "value (" << value << ") is two small. It should be at least: " - << minInflationValue(), - std::invalid_argument); - - // tan(alpha) = 2*halfLength/radius; - const FCL_REAL tan_alpha = 2 * halfLength / radius; - const FCL_REAL sin_alpha = tan_alpha / std::sqrt(1 + tan_alpha * tan_alpha); - const FCL_REAL top_inflation = value / sin_alpha; - const FCL_REAL bottom_inflation = value; - - const FCL_REAL new_lz = 2 * halfLength + top_inflation + bottom_inflation; - const FCL_REAL new_cz = (top_inflation + bottom_inflation) / 2.; - const FCL_REAL new_radius = new_lz / tan_alpha; - - return std::make_pair(Cone(new_radius, new_lz), - Transform3f(Vec3f(0., 0., new_cz))); - } - - private: - virtual bool isEqual(const CollisionGeometry& _other) const { - const Cone* other_ptr = dynamic_cast(&_other); - if (other_ptr == nullptr) return false; - const Cone& other = *other_ptr; - - return radius == other.radius && halfLength == other.halfLength && - getSweptSphereRadius() == other.getSweptSphereRadius(); - } - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/// @brief Cylinder along Z axis. -/// The cylinder is defined at its centroid. -class HPP_FCL_DLLAPI Cylinder : public ShapeBase { - public: - /// @brief Default constructor - Cylinder() {} - - Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) { - halfLength = lz_ / 2; - } - - Cylinder(const Cylinder& other) - : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {} - - Cylinder& operator=(const Cylinder& other) { - if (this == &other) return *this; - - this->radius = other.radius; - this->halfLength = other.halfLength; - return *this; - } - - /// @brief Clone *this into a new Cylinder - virtual Cylinder* clone() const { return new Cylinder(*this); }; - - /// @brief Radius of the cylinder - FCL_REAL radius; - - /// @brief Half Length along z axis - FCL_REAL halfLength; - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a cylinder - NODE_TYPE getNodeType() const { return GEOM_CYLINDER; } - - FCL_REAL computeVolume() const { - return boost::math::constants::pi() * radius * radius * - (halfLength * 2); - } - - Matrix3f computeMomentofInertia() const { - FCL_REAL V = computeVolume(); - FCL_REAL ix = V * (radius * radius / 4 + halfLength * halfLength / 3); - FCL_REAL iz = V * radius * radius / 2; - return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); - } - - FCL_REAL minInflationValue() const { return -(std::min)(radius, halfLength); } - - /// \brief Inflate the cylinder by an amount given by `value`. - /// This value can be positive or negative but must always >= - /// `minInflationValue()`. - /// - /// \param[in] value of the shape inflation. - /// - /// \returns a new inflated cylinder and the related transform to account for - /// the change of shape frame - std::pair inflated(const FCL_REAL value) const { - if (value <= minInflationValue()) - HPP_FCL_THROW_PRETTY( - "value (" << value << ") is two small. It should be at least: " - << minInflationValue(), - std::invalid_argument); - return std::make_pair(Cylinder(radius + value, 2 * (halfLength + value)), - Transform3f()); - } - - private: - virtual bool isEqual(const CollisionGeometry& _other) const { - const Cylinder* other_ptr = dynamic_cast(&_other); - if (other_ptr == nullptr) return false; - const Cylinder& other = *other_ptr; - - return radius == other.radius && halfLength == other.halfLength && - getSweptSphereRadius() == other.getSweptSphereRadius(); - } - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/// @brief Base for convex polytope. -/// @note Inherited classes are responsible for filling ConvexBase::neighbors; -class HPP_FCL_DLLAPI ConvexBase : public ShapeBase { - public: - /// @brief Build a convex hull based on Qhull library - /// and store the vertices and optionally the triangles - /// \param points, num_points the points whose convex hull should be computed. - /// \param keepTriangles if \c true, returns a Convex object which - /// contains the triangle of the shape. - /// \param qhullCommand the command sent to qhull. - /// - if \c keepTriangles is \c true, this parameter should include - /// "Qt". If \c NULL, "Qt" is passed to Qhull. - /// - if \c keepTriangles is \c false, an empty string is passed to - /// Qhull. - /// \note hpp-fcl must have been compiled with option \c HPP_FCL_HAS_QHULL set - /// to \c ON. - static ConvexBase* convexHull(std::shared_ptr>& points, - unsigned int num_points, bool keepTriangles, - const char* qhullCommand = NULL); - - // TODO(louis): put this method in private sometime in the future. - HPP_FCL_DEPRECATED static ConvexBase* convexHull( - const Vec3f* points, unsigned int num_points, bool keepTriangles, - const char* qhullCommand = NULL); - - virtual ~ConvexBase(); - - /// @brief Clone (deep copy). - /// This method is consistent with BVHModel `clone` method. - /// The copy constructor is called, which duplicates the data. - virtual ConvexBase* clone() const { return new ConvexBase(*this); } - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a convex polytope - NODE_TYPE getNodeType() const { return GEOM_CONVEX; } - -#ifdef HPP_FCL_HAS_QHULL - /// @brief Builds the double description of the convex polytope, i.e. the set - /// of hyperplanes which intersection form the polytope. - void buildDoubleDescription(); -#endif - - struct HPP_FCL_DLLAPI Neighbors { - unsigned char count_; - unsigned int* n_; - - unsigned char const& count() const { return count_; } - unsigned int& operator[](int i) { - assert(i < count_); - return n_[i]; - } - unsigned int const& operator[](int i) const { - assert(i < count_); - return n_[i]; - } - - bool operator==(const Neighbors& other) const { - if (count_ != other.count_) return false; - - for (int i = 0; i < count_; ++i) { - if (n_[i] != other.n_[i]) return false; - } - - return true; - } - - bool operator!=(const Neighbors& other) const { return !(*this == other); } - }; - - /// @brief Above this threshold, the convex polytope is considered large. - /// This influcences the way the support function is computed. - static constexpr size_t num_vertices_large_convex_threshold = 32; - - /// @brief An array of the points of the polygon. - std::shared_ptr> points; - unsigned int num_points; - - /// @brief An array of the normals of the polygon. - std::shared_ptr> normals; - /// @brief An array of the offsets to the normals of the polygon. - /// Note: there are as many offsets as normals. - std::shared_ptr> offsets; - unsigned int num_normals_and_offsets; - - /// @brief Neighbors of each vertex. - /// It is an array of size num_points. For each vertex, it contains the number - /// of neighbors and a list of indices pointing to them. - std::shared_ptr> neighbors; - - /// @brief center of the convex polytope, this is used for collision: center - /// is guaranteed in the internal of the polytope (as it is convex) - Vec3f center; - - /// @brief The support warm start polytope contains certain points of `this` - /// which are support points in specific directions of space. - /// This struct is used to warm start the support function computation for - /// large meshes (`num_points` > 32). - struct SupportWarmStartPolytope { - /// @brief Array of support points to warm start the support function - /// computation. - std::vector points; - - /// @brief Indices of the support points warm starts. - /// These are the indices of the real convex, not the indices of points in - /// the warm start polytope. - std::vector indices; - }; - - /// @brief Number of support warm starts. - static constexpr size_t num_support_warm_starts = 14; - - /// @brief Support warm start polytopes. - SupportWarmStartPolytope support_warm_starts; - - protected: - /// @brief Construct an uninitialized convex object - /// Initialization is done with ConvexBase::initialize. - ConvexBase() - : ShapeBase(), - num_points(0), - num_normals_and_offsets(0), - center(Vec3f::Zero()) {} - - /// @brief Initialize the points of the convex shape - /// This also initializes the ConvexBase::center. - /// - /// \param ownStorage weither the ConvexBase owns the data. - /// \param points_ list of 3D points /// - /// \param num_points_ number of 3D points - void initialize(std::shared_ptr> points_, - unsigned int num_points_); - - /// @brief Set the points of the convex shape. - /// - /// \param ownStorage weither the ConvexBase owns the data. - /// \param points_ list of 3D points /// - /// \param num_points_ number of 3D points - void set(std::shared_ptr> points_, - unsigned int num_points_); - - /// @brief Copy constructor - /// Only the list of neighbors is copied. - ConvexBase(const ConvexBase& other); - -#ifdef HPP_FCL_HAS_QHULL - void buildDoubleDescriptionFromQHullResult(const orgQhull::Qhull& qh); -#endif - - /// @brief Build the support points warm starts. - void buildSupportWarmStart(); - - /// @brief Array of indices of the neighbors of each vertex. - /// Since we don't know a priori the number of neighbors of each vertex, we - /// store the indices of the neighbors in a single array. - /// The `neighbors` attribute, an array of `Neighbors`, is used to point each - /// vertex to the right indices in the `nneighbors_` array. - std::shared_ptr> nneighbors_; - - private: - void computeCenter(); - - virtual bool isEqual(const CollisionGeometry& _other) const { - const ConvexBase* other_ptr = dynamic_cast(&_other); - if (other_ptr == nullptr) return false; - const ConvexBase& other = *other_ptr; - - if (num_points != other.num_points) return false; - - if ((!(points.get()) && other.points.get()) || - (points.get() && !(other.points.get()))) - return false; - if (points.get() && other.points.get()) { - const std::vector& points_ = *points; - const std::vector& other_points_ = *(other.points); - for (unsigned int i = 0; i < num_points; ++i) { - if (points_[i] != (other_points_)[i]) return false; - } - } - - if ((!(neighbors.get()) && other.neighbors.get()) || - (neighbors.get() && !(other.neighbors.get()))) - return false; - if (neighbors.get() && other.neighbors.get()) { - const std::vector& neighbors_ = *neighbors; - const std::vector& other_neighbors_ = *(other.neighbors); - for (unsigned int i = 0; i < num_points; ++i) { - if (neighbors_[i] != other_neighbors_[i]) return false; - } - } - - if ((!(normals.get()) && other.normals.get()) || - (normals.get() && !(other.normals.get()))) - return false; - if (normals.get() && other.normals.get()) { - const std::vector& normals_ = *normals; - const std::vector& other_normals_ = *(other.normals); - for (unsigned int i = 0; i < num_normals_and_offsets; ++i) { - if (normals_[i] != other_normals_[i]) return false; - } - } - - if ((!(offsets.get()) && other.offsets.get()) || - (offsets.get() && !(other.offsets.get()))) - return false; - if (offsets.get() && other.offsets.get()) { - const std::vector& offsets_ = *offsets; - const std::vector& other_offsets_ = *(other.offsets); - for (unsigned int i = 0; i < num_normals_and_offsets; ++i) { - if (offsets_[i] != other_offsets_[i]) return false; - } - } - - if (this->support_warm_starts.points.size() != - other.support_warm_starts.points.size() || - this->support_warm_starts.indices.size() != - other.support_warm_starts.indices.size()) { - return false; - } - - for (size_t i = 0; i < this->support_warm_starts.points.size(); ++i) { - if (this->support_warm_starts.points[i] != - other.support_warm_starts.points[i] || - this->support_warm_starts.indices[i] != - other.support_warm_starts.indices[i]) { - return false; - } - } - - return center == other.center && - getSweptSphereRadius() == other.getSweptSphereRadius(); - } - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -template -class Convex; - -/// @brief Half Space: this is equivalent to the Plane in ODE. -/// A Half space has a priviledged direction: the direction of the normal. -/// The separation plane is defined as n * x = d; Points in the negative side of -/// the separation plane (i.e. {x | n * x < d}) are inside the half space and -/// points in the positive side of the separation plane (i.e. {x | n * x > d}) -/// are outside the half space. -/// Note: prefer using a Halfspace instead of a Plane if possible, it has better -/// behavior w.r.t. collision detection algorithms. -class HPP_FCL_DLLAPI Halfspace : public ShapeBase { - public: - /// @brief Construct a half space with normal direction and offset - Halfspace(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) { - unitNormalTest(); - } - - /// @brief Construct a plane with normal direction and offset - Halfspace(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) - : ShapeBase(), n(a, b, c), d(d_) { - unitNormalTest(); - } - - Halfspace() : ShapeBase(), n(1, 0, 0), d(0) {} - - Halfspace(const Halfspace& other) - : ShapeBase(other), n(other.n), d(other.d) {} - - /// @brief operator = - Halfspace& operator=(const Halfspace& other) { - n = other.n; - d = other.d; - return *this; - } - - /// @brief Clone *this into a new Halfspace - virtual Halfspace* clone() const { return new Halfspace(*this); }; - - FCL_REAL signedDistance(const Vec3f& p) const { - return n.dot(p) - (d + this->getSweptSphereRadius()); - } - - FCL_REAL distance(const Vec3f& p) const { - return std::abs(this->signedDistance(p)); - } - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a half space - NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; } - - FCL_REAL minInflationValue() const { - return std::numeric_limits::lowest(); - } - - /// \brief Inflate the halfspace by an amount given by `value`. - /// This value can be positive or negative but must always >= - /// `minInflationValue()`. - /// - /// \param[in] value of the shape inflation. - /// - /// \returns a new inflated halfspace and the related transform to account for - /// the change of shape frame - std::pair inflated(const FCL_REAL value) const { - if (value <= minInflationValue()) - HPP_FCL_THROW_PRETTY( - "value (" << value << ") is two small. It should be at least: " - << minInflationValue(), - std::invalid_argument); - return std::make_pair(Halfspace(n, d + value), Transform3f()); - } - - /// @brief Plane normal - Vec3f n; - - /// @brief Plane offset - FCL_REAL d; - - protected: - /// @brief Turn non-unit normal into unit - void unitNormalTest(); - - private: - virtual bool isEqual(const CollisionGeometry& _other) const { - const Halfspace* other_ptr = dynamic_cast(&_other); - if (other_ptr == nullptr) return false; - const Halfspace& other = *other_ptr; - - return n == other.n && d == other.d && - getSweptSphereRadius() == other.getSweptSphereRadius(); - } - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/// @brief Infinite plane. -/// A plane can be viewed as two half spaces; it has no priviledged direction. -/// Note: prefer using a Halfspace instead of a Plane if possible, it has better -/// behavior w.r.t. collision detection algorithms. -class HPP_FCL_DLLAPI Plane : public ShapeBase { - public: - /// @brief Construct a plane with normal direction and offset - Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) { - unitNormalTest(); - } - - /// @brief Construct a plane with normal direction and offset - Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) - : ShapeBase(), n(a, b, c), d(d_) { - unitNormalTest(); - } - - Plane() : ShapeBase(), n(1, 0, 0), d(0) {} - - Plane(const Plane& other) : ShapeBase(other), n(other.n), d(other.d) {} - - /// @brief operator = - Plane& operator=(const Plane& other) { - n = other.n; - d = other.d; - return *this; - } - - /// @brief Clone *this into a new Plane - virtual Plane* clone() const { return new Plane(*this); }; - - FCL_REAL signedDistance(const Vec3f& p) const { - const FCL_REAL dist = n.dot(p) - d; - FCL_REAL signed_dist = - std::abs(n.dot(p) - d) - this->getSweptSphereRadius(); - if (dist >= 0) { - return signed_dist; - } - if (signed_dist >= 0) { - return -signed_dist; - } - return signed_dist; - } - - FCL_REAL distance(const Vec3f& p) const { - return std::abs(std::abs(n.dot(p) - d) - this->getSweptSphereRadius()); - } - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a plane - NODE_TYPE getNodeType() const { return GEOM_PLANE; } - - /// @brief Plane normal - Vec3f n; - - /// @brief Plane offset - FCL_REAL d; - - protected: - /// @brief Turn non-unit normal into unit - void unitNormalTest(); - - private: - virtual bool isEqual(const CollisionGeometry& _other) const { - const Plane* other_ptr = dynamic_cast(&_other); - if (other_ptr == nullptr) return false; - const Plane& other = *other_ptr; - - return n == other.n && d == other.d && - getSweptSphereRadius() == other.getSweptSphereRadius(); - } - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/shape/geometric_shapes_traits.h b/include/hpp/fcl/shape/geometric_shapes_traits.h index e4997d09a..3afa55f58 100644 --- a/include/hpp/fcl/shape/geometric_shapes_traits.h +++ b/include/hpp/fcl/shape/geometric_shapes_traits.h @@ -1,160 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * Copyright (c) 2015-2022, CNRS, Inria - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef HPP_FCL_GEOMETRIC_SHAPES_TRAITS_H -#define HPP_FCL_GEOMETRIC_SHAPES_TRAITS_H - -#include - -namespace hpp { -namespace fcl { - -struct shape_traits_base { - enum { - NeedNormalizedDir = false, - NeedNesterovNormalizeHeuristic = false, - IsInflatable = false, - HasInflatedSupportFunction = false, - IsStrictlyConvex = false - }; -}; - -template -struct shape_traits : shape_traits_base {}; - -template <> -struct shape_traits : shape_traits_base { - enum { - NeedNormalizedDir = false, - NeedNesterovNormalizeHeuristic = false, - IsInflatable = true, - HasInflatedSupportFunction = false, - IsStrictlyConvex = false - }; -}; - -template <> -struct shape_traits : shape_traits_base { - enum { - NeedNormalizedDir = false, - NeedNesterovNormalizeHeuristic = false, - IsInflatable = true, - HasInflatedSupportFunction = false, - IsStrictlyConvex = false - }; -}; - -template <> -struct shape_traits : shape_traits_base { - enum { - NeedNormalizedDir = false, - NeedNesterovNormalizeHeuristic = false, - IsInflatable = true, - HasInflatedSupportFunction = false, - IsStrictlyConvex = true - }; -}; - -template <> -struct shape_traits : shape_traits_base { - enum { - NeedNormalizedDir = false, - NeedNesterovNormalizeHeuristic = false, - IsInflatable = true, - HasInflatedSupportFunction = false, - IsStrictlyConvex = true - }; -}; - -template <> -struct shape_traits : shape_traits_base { - enum { - NeedNormalizedDir = false, - NeedNesterovNormalizeHeuristic = false, - IsInflatable = true, - HasInflatedSupportFunction = false, - IsStrictlyConvex = false - }; -}; - -template <> -struct shape_traits : shape_traits_base { - enum { - NeedNormalizedDir = false, - NeedNesterovNormalizeHeuristic = false, - IsInflatable = true, - HasInflatedSupportFunction = false, - IsStrictlyConvex = false - }; -}; - -template <> -struct shape_traits : shape_traits_base { - enum { - NeedNormalizedDir = false, - NeedNesterovNormalizeHeuristic = false, - IsInflatable = true, - HasInflatedSupportFunction = false, - IsStrictlyConvex = false - }; -}; - -template <> -struct shape_traits : shape_traits_base { - enum { - NeedNormalizedDir = false, - NeedNesterovNormalizeHeuristic = true, - IsInflatable = true, - HasInflatedSupportFunction = true, - IsStrictlyConvex = false - }; -}; - -template <> -struct shape_traits : shape_traits_base { - enum { - NeedNormalizedDir = false, - NeedNesterovNormalizeHeuristic = false, - IsInflatable = true, - HasInflatedSupportFunction = false, - IsStrictlyConvex = false - }; -}; - -} // namespace fcl -} // namespace hpp - -#endif // ifndef HPP_FCL_GEOMETRIC_SHAPES_TRAITS_H +#include +#include diff --git a/include/hpp/fcl/shape/geometric_shapes_utility.h b/include/hpp/fcl/shape/geometric_shapes_utility.h index f46ce042e..65e33d534 100644 --- a/include/hpp/fcl/shape/geometric_shapes_utility.h +++ b/include/hpp/fcl/shape/geometric_shapes_utility.h @@ -1,265 +1,2 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_GEOMETRIC_SHAPES_UTILITY_H -#define HPP_FCL_GEOMETRIC_SHAPES_UTILITY_H - -#include -#include -#include -#include - -namespace hpp { -namespace fcl { - -/// @cond IGNORE -namespace details { -/// @brief get the vertices of some convex shape which can bound the given shape -/// in a specific configuration -HPP_FCL_DLLAPI std::vector getBoundVertices(const Box& box, - const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const Sphere& sphere, - const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const Ellipsoid& ellipsoid, - const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const Capsule& capsule, - const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const Cone& cone, - const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const Cylinder& cylinder, - const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const ConvexBase& convex, - const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const TriangleP& triangle, - const Transform3f& tf); -} // namespace details -/// @endcond - -/// @brief calculate a bounding volume for a shape in a specific configuration -template -inline void computeBV(const S& s, const Transform3f& tf, BV& bv) { - if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); - } - std::vector convex_bound_vertices = details::getBoundVertices(s, tf); - fit(&convex_bound_vertices[0], (unsigned int)convex_bound_vertices.size(), - bv); -} - -template <> -HPP_FCL_DLLAPI void computeBV(const Box& s, const Transform3f& tf, - AABB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Sphere& s, - const Transform3f& tf, AABB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Ellipsoid& e, - const Transform3f& tf, AABB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Capsule& s, - const Transform3f& tf, AABB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Cone& s, const Transform3f& tf, - AABB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Cylinder& s, - const Transform3f& tf, AABB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const ConvexBase& s, - const Transform3f& tf, - AABB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const TriangleP& s, - const Transform3f& tf, AABB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Halfspace& s, - const Transform3f& tf, AABB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Plane& s, - const Transform3f& tf, AABB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Box& s, const Transform3f& tf, - OBB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Sphere& s, - const Transform3f& tf, OBB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Capsule& s, - const Transform3f& tf, OBB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Cone& s, const Transform3f& tf, - OBB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Cylinder& s, - const Transform3f& tf, OBB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const ConvexBase& s, - const Transform3f& tf, OBB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Halfspace& s, - const Transform3f& tf, OBB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Halfspace& s, - const Transform3f& tf, RSS& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Halfspace& s, - const Transform3f& tf, - OBBRSS& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Halfspace& s, - const Transform3f& tf, kIOS& bv); - -template <> -HPP_FCL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, - const Transform3f& tf, - KDOP<16>& bv); - -template <> -HPP_FCL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, - const Transform3f& tf, - KDOP<18>& bv); - -template <> -HPP_FCL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, - const Transform3f& tf, - KDOP<24>& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, - OBB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, - RSS& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Plane& s, - const Transform3f& tf, OBBRSS& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const Plane& s, - const Transform3f& tf, kIOS& bv); - -template <> -HPP_FCL_DLLAPI void computeBV, Plane>(const Plane& s, - const Transform3f& tf, - KDOP<16>& bv); - -template <> -HPP_FCL_DLLAPI void computeBV, Plane>(const Plane& s, - const Transform3f& tf, - KDOP<18>& bv); - -template <> -HPP_FCL_DLLAPI void computeBV, Plane>(const Plane& s, - const Transform3f& tf, - KDOP<24>& bv); - -/// @brief construct a box shape (with a configuration) from a given bounding -/// volume -HPP_FCL_DLLAPI void constructBox(const AABB& bv, Box& box, Transform3f& tf); - -HPP_FCL_DLLAPI void constructBox(const OBB& bv, Box& box, Transform3f& tf); - -HPP_FCL_DLLAPI void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf); - -HPP_FCL_DLLAPI void constructBox(const kIOS& bv, Box& box, Transform3f& tf); - -HPP_FCL_DLLAPI void constructBox(const RSS& bv, Box& box, Transform3f& tf); - -HPP_FCL_DLLAPI void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf); - -HPP_FCL_DLLAPI void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf); - -HPP_FCL_DLLAPI void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf); - -HPP_FCL_DLLAPI void constructBox(const AABB& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); - -HPP_FCL_DLLAPI void constructBox(const OBB& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); - -HPP_FCL_DLLAPI void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); - -HPP_FCL_DLLAPI void constructBox(const kIOS& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); - -HPP_FCL_DLLAPI void constructBox(const RSS& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); - -HPP_FCL_DLLAPI void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); - -HPP_FCL_DLLAPI void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); - -HPP_FCL_DLLAPI void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); - -HPP_FCL_DLLAPI Halfspace transform(const Halfspace& a, const Transform3f& tf); - -HPP_FCL_DLLAPI Plane transform(const Plane& a, const Transform3f& tf); - -HPP_FCL_DLLAPI std::array transformToHalfspaces( - const Plane& a, const Transform3f& tf); - -} // namespace fcl - -} // namespace hpp - -#endif +#include +#include diff --git a/include/hpp/fcl/timings.h b/include/hpp/fcl/timings.h index 961a26752..4500e9f19 100644 --- a/include/hpp/fcl/timings.h +++ b/include/hpp/fcl/timings.h @@ -1,114 +1,2 @@ -// -// Copyright (c) 2021-2023 INRIA -// - -#ifndef HPP_FCL_TIMINGS_FWD_H -#define HPP_FCL_TIMINGS_FWD_H - -#include "hpp/fcl/fwd.hh" - -#ifdef HPP_FCL_WITH_CXX11_SUPPORT -#include -#endif - -namespace hpp { -namespace fcl { - -struct CPUTimes { - double wall; - double user; - double system; - - CPUTimes() : wall(0), user(0), system(0) {} - - void clear() { wall = user = system = 0; } -}; - -/// -/// @brief This class mimics the way "boost/timer/timer.hpp" operates while -/// using the modern std::chrono library. -/// Importantly, this class will only have an effect for C++11 and more. -/// -struct HPP_FCL_DLLAPI Timer { -#ifdef HPP_FCL_WITH_CXX11_SUPPORT - typedef std::chrono::steady_clock clock_type; - typedef clock_type::duration duration_type; -#endif - - /// \brief Default constructor for the timer - /// - /// \param[in] start_on_construction if true, the timer will be run just after - /// the object is created - Timer(const bool start_on_construction = true) : m_is_stopped(true) { - if (start_on_construction) Timer::start(); - } - - CPUTimes elapsed() const { - if (m_is_stopped) return m_times; - - CPUTimes current(m_times); -#ifdef HPP_FCL_WITH_CXX11_SUPPORT - std::chrono::time_point current_clock = - std::chrono::steady_clock::now(); - current.user += static_cast( - std::chrono::duration_cast( - current_clock - m_start) - .count()) * - 1e-3; -#endif - return current; - } - -#ifdef HPP_FCL_WITH_CXX11_SUPPORT - duration_type duration() const { return (m_end - m_start); } -#endif - - void start() { - if (m_is_stopped) { - m_is_stopped = false; - m_times.clear(); - -#ifdef HPP_FCL_WITH_CXX11_SUPPORT - m_start = std::chrono::steady_clock::now(); -#endif - } - } - - void stop() { - if (m_is_stopped) return; - m_is_stopped = true; - -#ifdef HPP_FCL_WITH_CXX11_SUPPORT - m_end = std::chrono::steady_clock::now(); - m_times.user += static_cast( - std::chrono::duration_cast( - m_end - m_start) - .count()) * - 1e-3; -#endif - } - - void resume() { -#ifdef HPP_FCL_WITH_CXX11_SUPPORT - if (m_is_stopped) { - m_start = std::chrono::steady_clock::now(); - m_is_stopped = false; - } -#endif - } - - bool is_stopped() const { return m_is_stopped; } - - protected: - CPUTimes m_times; - bool m_is_stopped; - -#ifdef HPP_FCL_WITH_CXX11_SUPPORT - std::chrono::time_point m_start, m_end; -#endif -}; - -} // namespace fcl -} // namespace hpp - -#endif // ifndef HPP_FCL_TIMINGS_FWD_H +#include +#include diff --git a/include/hpp/fcl/warning.hh b/include/hpp/fcl/warning.hh new file mode 100644 index 000000000..c49b82715 --- /dev/null +++ b/include/hpp/fcl/warning.hh @@ -0,0 +1,2 @@ +#include +#include diff --git a/package.xml b/package.xml index 216b4a192..0428aa2f2 100644 --- a/package.xml +++ b/package.xml @@ -7,6 +7,7 @@ Please check the repository URL for full list of authors and maintainers. --> Joseph Mirabel Justin Carpentier + Louis Montaut Wolfgang Merkt BSD diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index a505d0dfa..38b98031b 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -35,15 +35,15 @@ include(${JRL_CMAKE_MODULES}/python-helpers.cmake) include(${JRL_CMAKE_MODULES}/stubs.cmake) -ADD_CUSTOM_TARGET(python) -SET_TARGET_PROPERTIES(python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True) +ADD_CUSTOM_TARGET(${PROJECT_NAME}_python) +SET_TARGET_PROPERTIES(${PROJECT_NAME}_python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True) # Name of the Python library -SET(LIBRARY_NAME hppfcl) +SET(PYTHON_LIB_NAME ${PROJECT_NAME}_pywrap) -SET(${LIBRARY_NAME}_HEADERS +SET(${PYTHON_LIB_NAME}_HEADERS fwd.hh - fcl.hh + coal.hh deprecation.hh broadphase/fwd.hh broadphase/broadphase_collision_manager.hh @@ -61,6 +61,14 @@ IF( NOT ENABLE_PYTHON_DOXYGEN_AUTODOC ELSE() SET(ENABLE_DOXYGEN_AUTODOC TRUE) + IF(DOXYGEN_VERSION VERSION_GREATER_EQUAL 1.9.8 AND DOXYGEN_VERSION VERSION_LESS 1.11.0) + # deactivate python doxygen automoc for doxygen 1.9.8 and 1.10.0, + # as it incorrectly parse "const" keyword, + # generating wrong links in C++ doc, and fail generating python doc + # ref. https://github.com/doxygen/doxygen/issues/10797 + SET(ENABLE_DOXYGEN_AUTODOC FALSE) + MESSAGE(AUTHOR_WARNING "automoc deactivated because of doxygen 1.10. Please use <1.10 or >=1.11.") + ENDIF() EXECUTE_PROCESS(COMMAND ${PYTHON_EXECUTABLE} -c "import lxml" RESULT_VARIABLE _pypkg_found OUTPUT_QUIET @@ -86,8 +94,8 @@ ELSE() ENDIF() IF(ENABLE_DOXYGEN_AUTODOC) ADD_CUSTOM_TARGET(generate_doxygen_cpp_doc - COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/python/doxygen_xml_parser.py - ${CMAKE_BINARY_DIR}/doc/doxygen-xml/index.xml + COMMAND ${PYTHON_EXECUTABLE} ${PROJECT_SOURCE_DIR}/doc/python/doxygen_xml_parser.py + ${PROJECT_BINARY_DIR}/doc/doxygen-xml/index.xml ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc > ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc.log BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc/doxygen_xml_parser_for_cmake.hh @@ -96,52 +104,54 @@ IF(ENABLE_DOXYGEN_AUTODOC) ) ADD_DEPENDENCIES(generate_doxygen_cpp_doc doc) - LIST(APPEND ${LIBRARY_NAME}_HEADERS + LIST(APPEND ${PYTHON_LIB_NAME}_HEADERS ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc/doxygen_xml_parser_for_cmake.hh ) ENDIF() -SET(${LIBRARY_NAME}_SOURCES +SET(${PYTHON_LIB_NAME}_SOURCES version.cc math.cc collision-geometries.cc collision.cc contact_patch.cc distance.cc - fcl.cc + coal.cc gjk.cc broadphase/broadphase.cc ) -IF(HPP_FCL_HAS_OCTOMAP) - LIST(APPEND ${LIBRARY_NAME}_SOURCES octree.cc) -ENDIF(HPP_FCL_HAS_OCTOMAP) +IF(COAL_HAS_OCTOMAP) + LIST(APPEND ${PYTHON_LIB_NAME}_SOURCES octree.cc) +ENDIF(COAL_HAS_OCTOMAP) -ADD_LIBRARY(${LIBRARY_NAME} MODULE ${${LIBRARY_NAME}_SOURCES} ${${LIBRARY_NAME}_HEADERS}) -TARGET_INCLUDE_DIRECTORIES(${LIBRARY_NAME} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIRS}) -TARGET_INCLUDE_DIRECTORIES(${LIBRARY_NAME} PRIVATE "${CMAKE_SOURCE_DIR}/src" "${CMAKE_CURRENT_BINARY_DIR}") +ADD_LIBRARY(${PYTHON_LIB_NAME} MODULE ${${PYTHON_LIB_NAME}_SOURCES} ${${PYTHON_LIB_NAME}_HEADERS}) +TARGET_INCLUDE_DIRECTORIES(${PYTHON_LIB_NAME} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIRS}) +TARGET_INCLUDE_DIRECTORIES(${PYTHON_LIB_NAME} PRIVATE "${PROJECT_SOURCE_DIR}/src" "${CMAKE_CURRENT_BINARY_DIR}") IF(WIN32) - TARGET_LINK_LIBRARIES(${LIBRARY_NAME} PUBLIC ${PYTHON_LIBRARY}) + TARGET_LINK_LIBRARIES(${PYTHON_LIB_NAME} PUBLIC ${PYTHON_LIBRARY}) ENDIF(WIN32) -ADD_DEPENDENCIES(python ${LIBRARY_NAME}) -ADD_HEADER_GROUP(${LIBRARY_NAME}_HEADERS) -ADD_SOURCE_GROUP(${LIBRARY_NAME}_SOURCES) +ADD_DEPENDENCIES(${PROJECT_NAME}_python ${PYTHON_LIB_NAME}) +ADD_HEADER_GROUP(${PYTHON_LIB_NAME}_HEADERS) +ADD_SOURCE_GROUP(${PYTHON_LIB_NAME}_SOURCES) IF(ENABLE_DOXYGEN_AUTODOC) - ADD_DEPENDENCIES(${LIBRARY_NAME} generate_doxygen_cpp_doc) - TARGET_COMPILE_DEFINITIONS(${LIBRARY_NAME} PRIVATE -DHPP_FCL_HAS_DOXYGEN_AUTODOC) + ADD_DEPENDENCIES(${PYTHON_LIB_NAME} generate_doxygen_cpp_doc) + TARGET_COMPILE_DEFINITIONS(${PYTHON_LIB_NAME} PRIVATE -DCOAL_HAS_DOXYGEN_AUTODOC) ENDIF() -TARGET_LINK_LIBRARIES(${LIBRARY_NAME} PUBLIC +TARGET_LINK_LIBRARIES(${PYTHON_LIB_NAME} PUBLIC ${PROJECT_NAME} eigenpy::eigenpy Boost::system) -SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES +SET_TARGET_PROPERTIES(${PYTHON_LIB_NAME} PROPERTIES PREFIX "" SUFFIX "${PYTHON_EXT_SUFFIX}" - LIBRARY_OUTPUT_NAME ${LIBRARY_NAME} - LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/python/${LIBRARY_NAME}" + OUTPUT_NAME "${PYTHON_LIB_NAME}" + LIBRARY_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/python/${PROJECT_NAME}" + # On Windows, shared library are treat as binary + RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/python/${PROJECT_NAME}" ) IF(IS_ABSOLUTE ${PYTHON_SITELIB}) @@ -149,30 +159,38 @@ IF(IS_ABSOLUTE ${PYTHON_SITELIB}) ELSE() SET(ABSOLUTE_PYTHON_SITELIB ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITELIB}) ENDIF() -SET(${LIBRARY_NAME}_INSTALL_DIR ${ABSOLUTE_PYTHON_SITELIB}/${LIBRARY_NAME}) +SET(${PYTHON_LIB_NAME}_INSTALL_DIR ${ABSOLUTE_PYTHON_SITELIB}/${PROJECT_NAME}) IF(UNIX) - GET_RELATIVE_RPATH(${${LIBRARY_NAME}_INSTALL_DIR} ${LIBRARY_NAME}_INSTALL_RPATH) - SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES INSTALL_RPATH "${${LIBRARY_NAME}_INSTALL_RPATH}") + GET_RELATIVE_RPATH(${${PYTHON_LIB_NAME}_INSTALL_DIR} ${PYTHON_LIB_NAME}_INSTALL_RPATH) + SET_TARGET_PROPERTIES(${PYTHON_LIB_NAME} PROPERTIES INSTALL_RPATH "${${PYTHON_LIB_NAME}_INSTALL_RPATH}") ENDIF() -INSTALL(TARGETS ${LIBRARY_NAME} - DESTINATION ${${LIBRARY_NAME}_INSTALL_DIR}) +INSTALL(TARGETS ${PYTHON_LIB_NAME} + EXPORT ${TARGETS_EXPORT_NAME} + DESTINATION ${${PYTHON_LIB_NAME}_INSTALL_DIR}) # --- GENERATE STUBS IF(GENERATE_PYTHON_STUBS) LOAD_STUBGEN() - GENERATE_STUBS(${CMAKE_CURRENT_BINARY_DIR} ${LIBRARY_NAME} ${ABSOLUTE_PYTHON_SITELIB} ${LIBRARY_NAME} ${PROJECT_NAME}) + GENERATE_STUBS(${CMAKE_CURRENT_BINARY_DIR} ${PROJECT_NAME} ${ABSOLUTE_PYTHON_SITELIB} ${PYTHON_LIB_NAME} ${PROJECT_NAME}) ENDIF(GENERATE_PYTHON_STUBS) # --- INSTALL SCRIPTS SET(PYTHON_FILES __init__.py viewer.py + windows_dll_manager.py ) FOREACH(python ${PYTHON_FILES}) - PYTHON_BUILD(${LIBRARY_NAME} ${python}) + PYTHON_BUILD(${PROJECT_NAME} ${python}) INSTALL(FILES - "${CMAKE_CURRENT_SOURCE_DIR}/${LIBRARY_NAME}/${python}" - DESTINATION ${${LIBRARY_NAME}_INSTALL_DIR}) + "${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}/${python}" + DESTINATION ${${PYTHON_LIB_NAME}_INSTALL_DIR}) ENDFOREACH(python) + + +if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) + python_install_on_site(hppfcl __init__.py COMPONENT hpp-fcl-compatibility) + python_install_on_site(hppfcl viewer.py COMPONENT hpp-fcl-compatibility) +endif() diff --git a/python/broadphase/broadphase.cc b/python/broadphase/broadphase.cc index f6f9592ae..28e427a27 100644 --- a/python/broadphase/broadphase.cc +++ b/python/broadphase/broadphase.cc @@ -32,41 +32,41 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include -#include "../fcl.hh" +#include "coal/fwd.hh" +#include "../coal.hh" #include "../utils/std-pair.hh" -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" -#include "hpp/fcl/broadphase/broadphase_bruteforce.h" -#include "hpp/fcl/broadphase/broadphase_SaP.h" -#include "hpp/fcl/broadphase/broadphase_SSaP.h" -#include "hpp/fcl/broadphase/broadphase_interval_tree.h" -#include "hpp/fcl/broadphase/broadphase_spatialhash.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "coal/broadphase/broadphase_bruteforce.h" +#include "coal/broadphase/broadphase_SaP.h" +#include "coal/broadphase/broadphase_SSaP.h" +#include "coal/broadphase/broadphase_interval_tree.h" +#include "coal/broadphase/broadphase_spatialhash.h" -HPP_FCL_COMPILER_DIAGNOSTIC_PUSH -HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +COAL_COMPILER_DIAGNOSTIC_PUSH +COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" -HPP_FCL_COMPILER_DIAGNOSTIC_POP -#include "doxygen_autodoc/hpp/fcl/broadphase/default_broadphase_callbacks.h" -// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" +COAL_COMPILER_DIAGNOSTIC_POP +#include "doxygen_autodoc/coal/broadphase/default_broadphase_callbacks.h" +// #include "doxygen_autodoc/coal/broadphase/broadphase_dynamic_AABB_tree.h" // #include -//"doxygen_autodoc/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" -// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_bruteforce.h" -// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_SaP.h" -// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_SSaP.h" -// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_interval_tree.h" -// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_spatialhash.h" +//"doxygen_autodoc/coal/broadphase/broadphase_dynamic_AABB_tree_array.h" +// #include "doxygen_autodoc/coal/broadphase/broadphase_bruteforce.h" +// #include "doxygen_autodoc/coal/broadphase/broadphase_SaP.h" +// #include "doxygen_autodoc/coal/broadphase/broadphase_SSaP.h" +// #include "doxygen_autodoc/coal/broadphase/broadphase_interval_tree.h" +// #include "doxygen_autodoc/coal/broadphase/broadphase_spatialhash.h" #endif #include "broadphase_callbacks.hh" #include "broadphase_collision_manager.hh" -using namespace hpp::fcl; +using namespace coal; -HPP_FCL_COMPILER_DIAGNOSTIC_PUSH -HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS +COAL_COMPILER_DIAGNOSTIC_PUSH +COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS void exposeBroadPhase() { CollisionCallBackBaseWrapper::expose(); DistanceCallBackBaseWrapper::expose(); @@ -135,8 +135,8 @@ void exposeBroadPhase() { typedef SpatialHashingCollisionManager Derived; bp::class_>( "SpatialHashingCollisionManager", bp::no_init) - .def(dv::init>()); } } -HPP_FCL_COMPILER_DIAGNOSTIC_POP +COAL_COMPILER_DIAGNOSTIC_POP diff --git a/python/broadphase/broadphase_callbacks.hh b/python/broadphase/broadphase_callbacks.hh index b6ed07d7b..b0ae02d30 100644 --- a/python/broadphase/broadphase_callbacks.hh +++ b/python/broadphase/broadphase_callbacks.hh @@ -32,23 +32,22 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#ifndef HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH -#define HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH +#ifndef COAL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH +#define COAL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH #include -#include -#include +#include "coal/fwd.hh" +#include "coal/broadphase/broadphase_callbacks.h" -#include "../fcl.hh" +#include "../coal.hh" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" -#include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_callbacks.h" +#include "doxygen_autodoc/coal/broadphase/broadphase_callbacks.h" #endif -namespace hpp { -namespace fcl { +namespace coal { struct CollisionCallBackBaseWrapper : CollisionCallBackBase, bp::wrapper { @@ -85,7 +84,7 @@ struct DistanceCallBackBaseWrapper : DistanceCallBackBase, return distance(o1, o2, dist.coeffRef(0, 0)); } - bool distance(CollisionObject* o1, CollisionObject* o2, FCL_REAL& dist) { + bool distance(CollisionObject* o1, CollisionObject* o2, CoalScalar& dist) { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wconversion" return this->get_override("distance")(o1, o2, dist); @@ -108,7 +107,6 @@ struct DistanceCallBackBaseWrapper : DistanceCallBackBase, } }; // DistanceCallBackBaseWrapper -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH +#endif // ifndef COAL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH diff --git a/python/broadphase/broadphase_collision_manager.hh b/python/broadphase/broadphase_collision_manager.hh index 796dbd475..2624c0dcb 100644 --- a/python/broadphase/broadphase_collision_manager.hh +++ b/python/broadphase/broadphase_collision_manager.hh @@ -32,27 +32,26 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#ifndef HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH -#define HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH +#ifndef COAL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH +#define COAL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH #include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/broadphase/broadphase_collision_manager.h" +#include "coal/broadphase/default_broadphase_callbacks.h" -#include "../fcl.hh" +#include "../coal.hh" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" -#include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_collision_manager.h" +#include "doxygen_autodoc/coal/broadphase/broadphase_collision_manager.h" #endif #include #include -namespace hpp { -namespace fcl { +namespace coal { struct BroadPhaseCollisionManagerWrapper : BroadPhaseCollisionManager, @@ -216,7 +215,7 @@ struct BroadPhaseCollisionManagerWrapper template static void exposeDerived() { std::string class_name = boost::typeindex::type_id().pretty_name(); - boost::algorithm::replace_all(class_name, "hpp::fcl::", ""); + boost::algorithm::replace_all(class_name, "coal::", ""); #if defined(WIN32) boost::algorithm::replace_all(class_name, "class ", ""); #endif @@ -228,7 +227,6 @@ struct BroadPhaseCollisionManagerWrapper }; // BroadPhaseCollisionManagerWrapper -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH +#endif // ifndef COAL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH diff --git a/python/broadphase/fwd.hh b/python/broadphase/fwd.hh index 2f8e6bc57..1367cc065 100644 --- a/python/broadphase/fwd.hh +++ b/python/broadphase/fwd.hh @@ -2,19 +2,17 @@ // Copyright (c) 2022 INRIA // -#ifndef HPP_FCL_PYTHON_BROADPHASE_FWD_HH -#define HPP_FCL_PYTHON_BROADPHASE_FWD_HH +#ifndef COAL_PYTHON_BROADPHASE_FWD_HH +#define COAL_PYTHON_BROADPHASE_FWD_HH -#include "hppfcl/fwd.hh" +#include "coal/fwd.hh" -namespace hpp { -namespace fcl { +namespace coal { namespace python { void exposeBroadPhase(); } -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_PYTHON_BROADPHASE_FWD_HH +#endif // ifndef COAL_PYTHON_BROADPHASE_FWD_HH diff --git a/python/fcl.cc b/python/coal.cc similarity index 90% rename from python/fcl.cc rename to python/coal.cc index 956ac8715..dee8825d1 100644 --- a/python/fcl.cc +++ b/python/coal.cc @@ -34,24 +34,24 @@ #include -#include "fcl.hh" +#include "coal.hh" -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/shape/geometric_shapes.h" +#include "coal/BVH/BVH_model.h" -#include +#include "coal/mesh_loader/loader.h" -#include +#include "coal/collision.h" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC -#include "doxygen_autodoc/hpp/fcl/mesh_loader/loader.h" +#ifdef COAL_HAS_DOXYGEN_AUTODOC +#include "doxygen_autodoc/coal/mesh_loader/loader.h" #endif #include "../doc/python/doxygen.hh" #include "../doc/python/doxygen-boost.hh" -using namespace hpp::fcl; +using namespace coal; namespace dv = doxygen::visitor; #pragma GCC diagnostic push @@ -83,7 +83,7 @@ void exposeMeshLoader() { } } -BOOST_PYTHON_MODULE(hppfcl) { +BOOST_PYTHON_MODULE(coal_pywrap) { namespace bp = boost::python; PyImport_ImportModule("warnings"); @@ -98,7 +98,7 @@ BOOST_PYTHON_MODULE(hppfcl) { exposeContactPatchAPI(); exposeDistanceAPI(); exposeGJK(); -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP exposeOctree(); #endif exposeBroadPhase(); diff --git a/python/fcl.hh b/python/coal.hh similarity index 92% rename from python/fcl.hh rename to python/coal.hh index 6b06095f9..0dda80244 100644 --- a/python/fcl.hh +++ b/python/coal.hh @@ -18,7 +18,7 @@ void exposeDistanceAPI(); void exposeGJK(); -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP void exposeOctree(); #endif diff --git a/python/coal/__init__.py b/python/coal/__init__.py new file mode 100644 index 000000000..966642738 --- /dev/null +++ b/python/coal/__init__.py @@ -0,0 +1,61 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2019 INRIA +# Author: Justin Carpentier +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of CNRS-LAAS. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# ruff: noqa: F401, F403 + +# On Windows, if coal.dll is not in the same directory than +# the .pyd, it will not be loaded. +# We first try to load coal, then, if it fail and we are on Windows: +# 1. We add all paths inside COAL_WINDOWS_DLL_PATH to DllDirectory +# 2. If COAL_WINDOWS_DLL_PATH we add the relative path from the +# package directory to the bin directory to DllDirectory +# This solution is inspired from: +# - https://github.com/PixarAnimationStudios/OpenUSD/pull/1511/files +# - https://stackoverflow.com/questions/65334494/python-c-extension-packaging-dll-along-with-pyd +# More resources on https://github.com/diffpy/pyobjcryst/issues/33 +try: + from .coal_pywrap import * # noqa + from .coal_pywrap import __raw_version__, __version__ +except ImportError: + import platform + + if platform.system() == "Windows": + from .windows_dll_manager import build_directory_manager, get_dll_paths + + with build_directory_manager() as dll_dir_manager: + for p in get_dll_paths(): + dll_dir_manager.add_dll_directory(p) + from .coal_pywrap import * # noqa + from .coal_pywrap import __raw_version__, __version__ # noqa + else: + raise diff --git a/python/coal/viewer.py b/python/coal/viewer.py new file mode 100644 index 000000000..bef54e2c0 --- /dev/null +++ b/python/coal/viewer.py @@ -0,0 +1,110 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2019 CNRS +# Author: Joseph Mirabel + +import warnings + +import numpy as np +from gepetto import Color + +import coal + + +def applyConfiguration(gui, name, tf): + gui.applyConfiguration( + name, tf.getTranslation().tolist() + tf.getQuatRotation().coeffs().tolist() + ) + + +def displayShape(gui, name, geom, color=(0.9, 0.9, 0.9, 1.0)): + if isinstance(geom, coal.Capsule): + return gui.addCapsule(name, geom.radius, 2.0 * geom.halfLength, color) + elif isinstance(geom, coal.Cylinder): + return gui.addCylinder(name, geom.radius, 2.0 * geom.halfLength, color) + elif isinstance(geom, coal.Box): + w, h, d = (2.0 * geom.halfSide).tolist() + return gui.addBox(name, w, h, d, color) + elif isinstance(geom, coal.Sphere): + return gui.addSphere(name, geom.radius, color) + elif isinstance(geom, coal.Cone): + return gui.addCone(name, geom.radius, 2.0 * geom.halfLength, color) + elif isinstance(geom, coal.Convex): + pts = [ + geom.points(geom.polygons(f)[i]).tolist() + for f in range(geom.num_polygons) + for i in range(3) + ] + gui.addCurve(name, pts, color) + gui.setCurveMode(name, "TRIANGLES") + gui.setLightingMode(name, "ON") + gui.setBoolProperty(name, "BackfaceDrawing", True) + return True + elif isinstance(geom, coal.ConvexBase): + pts = [geom.points(i).tolist() for i in range(geom.num_points)] + gui.addCurve(name, pts, color) + gui.setCurveMode(name, "POINTS") + gui.setLightingMode(name, "OFF") + return True + else: + msg = "Unsupported geometry type for %s (%s)" % (name, type(geom)) + warnings.warn(msg, category=UserWarning, stacklevel=2) + return False + + +def displayDistanceResult(gui, group_name, res, closest_points=True, normal=True): + gui.createGroup(group_name) + r = 0.01 + if closest_points: + p = [group_name + "/p1", group_name + "/p2"] + gui.addSphere(p[0], r, Color.red) + gui.addSphere(p[1], r, Color.blue) + qid = [0, 0, 0, 1] + gui.applyConfigurations( + p, + [ + res.getNearestPoint1().tolist() + qid, + res.getNearestPoint2().tolist() + qid, + ], + ) + if normal: + n = group_name + "/normal" + gui.addArrow(n, r, 0.1, Color.green) + gui.applyConfiguration( + n, + res.getNearestPoint1().tolist() + + coal.Quaternion.FromTwoVectors(np.array([1, 0, 0]), res.normal) + .coeffs() + .tolist(), + ) + gui.refresh() + + +def displayCollisionResult(gui, group_name, res, color=Color.green): + if res.isCollision(): + if gui.nodeExists(group_name): + gui.setVisibility(group_name, "ON") + else: + gui.createGroup(group_name) + for i in range(res.numContacts()): + contact = res.getContact(i) + n = group_name + "/contact" + str(i) + depth = contact.penetration_depth + if gui.nodeExists(n): + gui.setFloatProperty(n, "Size", depth) + gui.setFloatProperty(n, "Radius", 0.1 * depth) + gui.setColor(n, color) + else: + gui.addArrow(n, depth * 0.1, depth, color) + N = contact.normal + P = contact.pos + gui.applyConfiguration( + n, + (P - depth * N / 2).tolist() + + coal.Quaternion.FromTwoVectors(np.array([1, 0, 0]), N) + .coeffs() + .tolist(), + ) + gui.refresh() + elif gui.nodeExists(group_name): + gui.setVisibility(group_name, "OFF") diff --git a/python/coal/windows_dll_manager.py b/python/coal/windows_dll_manager.py new file mode 100644 index 000000000..92516fee1 --- /dev/null +++ b/python/coal/windows_dll_manager.py @@ -0,0 +1,65 @@ +import contextlib +import os +import sys + + +def get_dll_paths(): + coal_paths = os.getenv("COAL_WINDOWS_DLL_PATH") + if coal_paths is None: + # From https://peps.python.org/pep-0250/#implementation + # lib/python-version/site-packages/package + RELATIVE_DLL_PATH1 = "..\\..\\..\\..\\bin" + # lib/site-packages/package + RELATIVE_DLL_PATH2 = "..\\..\\..\\bin" + # For unit test + RELATIVE_DLL_PATH3 = "..\\..\\bin" + return [ + os.path.join(os.path.dirname(__file__), RELATIVE_DLL_PATH1), + os.path.join(os.path.dirname(__file__), RELATIVE_DLL_PATH2), + os.path.join(os.path.dirname(__file__), RELATIVE_DLL_PATH3), + ] + else: + return coal_paths.split(os.pathsep) + + +class PathManager(contextlib.AbstractContextManager): + """Restore PATH state after importing Python module""" + + def add_dll_directory(self, dll_dir: str): + os.environ["PATH"] += os.pathsep + dll_dir + + def __enter__(self): + self.old_path = os.environ["PATH"] + return self + + def __exit__(self, *exc_details): + os.environ["PATH"] = self.old_path + + +class DllDirectoryManager(contextlib.AbstractContextManager): + """Restore DllDirectory state after importing Python module""" + + def add_dll_directory(self, dll_dir: str): + # add_dll_directory can fail on relative path and non + # existing path. + # Since we don't know all the fail criterion we just ignore + # thrown exception + try: + self.dll_dirs.append(os.add_dll_directory(dll_dir)) + except OSError: + pass + + def __enter__(self): + self.dll_dirs = [] + return self + + def __exit__(self, *exc_details): + for d in self.dll_dirs: + d.close() + + +def build_directory_manager(): + if sys.version_info >= (3, 8): + return DllDirectoryManager() + else: + return PathManager() diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index 3ee0a0b50..b3afc5594 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -1,7 +1,7 @@ // // Software License Agreement (BSD License) // -// Copyright (c) 2019-2022 CNRS-LAAS INRIA +// Copyright (c) 2019-2024 CNRS-LAAS INRIA // Author: Joseph Mirabel // All rights reserved. // @@ -34,50 +34,52 @@ #include #include + #if EIGENPY_VERSION_AT_LEAST(3, 8, 0) #include #endif -#include "fcl.hh" +#include "coal.hh" + #include "deprecation.hh" -#include -#include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/shape/geometric_shapes.h" +#include "coal/shape/convex.h" +#include "coal/BVH/BVH_model.h" +#include "coal/hfield.h" -#include -#include -#include -#include -#include -#include +#include "coal/serialization/memory.h" +#include "coal/serialization/AABB.h" +#include "coal/serialization/BVH_model.h" +#include "coal/serialization/hfield.h" +#include "coal/serialization/geometric_shapes.h" +#include "coal/serialization/convex.h" #include "pickle.hh" #include "serializable.hh" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC // FIXME for a reason I do not understand, doxygen fails to understand that -// BV_splitter is not defined in hpp/fcl/BVH/BVH_model.h -#include -#include - -#include "doxygen_autodoc/hpp/fcl/BVH/BVH_model.h" -#include "doxygen_autodoc/hpp/fcl/BV/AABB.h" -#include "doxygen_autodoc/hpp/fcl/hfield.h" -#include "doxygen_autodoc/hpp/fcl/shape/geometric_shapes.h" +// BV_splitter is not defined in coal/BVH/BVH_model.h +#include "coal/internal/BV_splitter.h" +#include "coal/broadphase/detail/hierarchy_tree.h" + +#include "doxygen_autodoc/coal/BVH/BVH_model.h" +#include "doxygen_autodoc/coal/BV/AABB.h" +#include "doxygen_autodoc/coal/hfield.h" +#include "doxygen_autodoc/coal/shape/geometric_shapes.h" #include "doxygen_autodoc/functions.h" #endif using namespace boost::python; -using namespace hpp::fcl; -using namespace hpp::fcl::python; +using namespace coal; +using namespace coal::python; namespace dv = doxygen::visitor; namespace bp = boost::python; using boost::noncopyable; -typedef std::vector Vec3fs; +typedef std::vector Vec3ss; typedef std::vector Triangles; struct BVHModelBaseWrapper { @@ -85,7 +87,7 @@ struct BVHModelBaseWrapper { typedef Eigen::Map MapRowMatrixX3; typedef Eigen::Ref RefRowMatrixX3; - static Vec3f& vertex(BVHModelBase& bvh, unsigned int i) { + static Vec3s& vertex(BVHModelBase& bvh, unsigned int i) { if (i >= bvh.num_vertices) throw std::out_of_range("index is out of range"); return (*(bvh.vertices))[i]; } @@ -136,8 +138,8 @@ void exposeHeightField(const std::string& bvname) { type_name.c_str(), doxygen::class_doc(), no_init) .def(dv::init>()) .def(dv::init, const HeightField&>()) - .def(dv::init, FCL_REAL, FCL_REAL, const MatrixXf&, - bp::optional>()) + .def(dv::init, CoalScalar, CoalScalar, const MatrixXs&, + bp::optional>()) .DEF_CLASS_FUNC(Geometry, getXDim) .DEF_CLASS_FUNC(Geometry, getYDim) @@ -178,7 +180,7 @@ struct ConvexBaseWrapper { typedef Eigen::Map MapVecOfDoubles; typedef Eigen::Ref RefVecOfDoubles; - static Vec3f& point(const ConvexBase& convex, unsigned int i) { + static Vec3s& point(const ConvexBase& convex, unsigned int i) { if (i >= convex.num_points) throw std::out_of_range("index is out of range"); return (*(convex.points))[i]; @@ -188,7 +190,7 @@ struct ConvexBaseWrapper { return MapRowMatrixX3((*(convex.points))[0].data(), convex.num_points, 3); } - static Vec3f& normal(const ConvexBase& convex, unsigned int i) { + static Vec3s& normal(const ConvexBase& convex, unsigned int i) { if (i >= convex.num_normals_and_offsets) throw std::out_of_range("index is out of range"); return (*(convex.normals))[i]; @@ -220,7 +222,7 @@ struct ConvexBaseWrapper { return n; } - static ConvexBase* convexHull(const Vec3fs& points, bool keepTri, + static ConvexBase* convexHull(const Vec3ss& points, bool keepTri, const char* qhullCommand) { return ConvexBase::convexHull(points.data(), (unsigned int)points.size(), keepTri, qhullCommand); @@ -237,11 +239,11 @@ struct ConvexWrapper { return (*convex.polygons)[i]; } - static shared_ptr constructor(const Vec3fs& _points, + static shared_ptr constructor(const Vec3ss& _points, const Triangles& _tris) { - std::shared_ptr> points( - new std::vector(_points.size())); - std::vector& points_ = *points; + std::shared_ptr> points( + new std::vector(_points.size())); + std::vector& points_ = *points; for (std::size_t i = 0; i < _points.size(); ++i) points_[i] = _points[i]; std::shared_ptr> tris( @@ -288,8 +290,8 @@ void exposeShapes() { "Box", doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) - .def(dv::init()) - .def(dv::init()) + .def(dv::init()) + .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Box, halfSide) .def("clone", &Box::clone, doxygen::member_func_doc(&Box::clone), return_value_policy()) @@ -303,7 +305,7 @@ void exposeShapes() { class_, shared_ptr>( "Capsule", doxygen::class_doc(), no_init) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Capsule, radius) .DEF_RW_CLASS_ATTRIB(Capsule, halfLength) @@ -319,7 +321,7 @@ void exposeShapes() { class_, shared_ptr>( "Cone", doxygen::class_doc(), no_init) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Cone, radius) .DEF_RW_CLASS_ATTRIB(Cone, halfLength) @@ -342,8 +344,7 @@ void exposeShapes() { bp::return_internal_reference<>()) .def("points", &ConvexBaseWrapper::point, bp::args("self", "index"), "Retrieve the point given by its index.", - ::hpp::fcl::python::deprecated_member< - bp::return_internal_reference<>>()) + ::coal::python::deprecated_member>()) .def("points", &ConvexBaseWrapper::points, bp::args("self"), "Retrieve all the points.", bp::with_custodian_and_ward_postcall<0, 1>()) @@ -387,7 +388,7 @@ void exposeShapes() { class_, shared_ptr>( "Cylinder", doxygen::class_doc(), no_init) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Cylinder, radius) .DEF_RW_CLASS_ATTRIB(Cylinder, halfLength) @@ -403,9 +404,10 @@ void exposeShapes() { class_, shared_ptr>( "Halfspace", doxygen::class_doc(), no_init) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) - .def(dv::init()) + .def( + dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Halfspace, n) .DEF_RW_CLASS_ATTRIB(Halfspace, d) @@ -421,9 +423,9 @@ void exposeShapes() { class_, shared_ptr>( "Plane", doxygen::class_doc(), no_init) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Plane, n) .DEF_RW_CLASS_ATTRIB(Plane, d) @@ -440,7 +442,7 @@ void exposeShapes() { "Sphere", doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Sphere, radius) .def("clone", &Sphere::clone, doxygen::member_func_doc(&Sphere::clone), return_value_policy()) @@ -454,8 +456,8 @@ void exposeShapes() { class_, shared_ptr>( "Ellipsoid", doxygen::class_doc(), no_init) .def(dv::init()) - .def(dv::init()) - .def(dv::init()) + .def(dv::init()) + .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Ellipsoid, radii) .def("clone", &Ellipsoid::clone, @@ -471,7 +473,7 @@ void exposeShapes() { class_, shared_ptr>( "TriangleP", doxygen::class_doc(), no_init) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(TriangleP, a) .DEF_RW_CLASS_ATTRIB(TriangleP, b) @@ -488,8 +490,8 @@ void exposeShapes() { } boost::python::tuple AABB_distance_proxy(const AABB& self, const AABB& other) { - Vec3f P, Q; - FCL_REAL distance = self.distance(other, &P, &Q); + Vec3s P, Q; + CoalScalar distance = self.distance(other, &P, &Q); return boost::python::make_tuple(distance, P, Q); } @@ -552,17 +554,17 @@ void exposeCollisionGeometries() { no_init) .def(init<>(bp::arg("self"), "Default constructor")) .def(init(bp::args("self", "other"), "Copy constructor")) - .def(init(bp::args("self", "v"), + .def(init(bp::args("self", "v"), "Creating an AABB at position v with zero size.")) - .def(init(bp::args("self", "a", "b"), + .def(init(bp::args("self", "a", "b"), "Creating an AABB with two endpoints a and b.")) - .def(init( + .def(init( bp::args("self", "core", "delta"), "Creating an AABB centered as core and is of half-dimension delta.")) - .def(init(bp::args("self", "a", "b", "c"), + .def(init(bp::args("self", "a", "b", "c"), "Creating an AABB contains three points.")) - .def("contain", (bool(AABB::*)(const Vec3f&) const) & AABB::contain, + .def("contain", (bool(AABB::*)(const Vec3s&) const) & AABB::contain, bp::args("self", "p"), "Check whether the AABB contains a point p.") .def("contain", (bool(AABB::*)(const AABB&) const) & AABB::contain, bp::args("self", "other"), @@ -575,7 +577,7 @@ void exposeCollisionGeometries() { "Check whether two AABB are overlaping and return the overloaping " "part if true.") - .def("distance", (FCL_REAL(AABB::*)(const AABB&) const)&AABB::distance, + .def("distance", (CoalScalar(AABB::*)(const AABB&) const)&AABB::distance, bp::args("self", "other"), "Distance between two AABBs.") // .def("distance", // AABB_distance_proxy, @@ -585,18 +587,18 @@ void exposeCollisionGeometries() { .add_property( "min_", bp::make_function( - +[](AABB& self) -> Vec3f& { return self.min_; }, + +[](AABB& self) -> Vec3s& { return self.min_; }, bp::return_internal_reference<>()), bp::make_function( - +[](AABB& self, const Vec3f& min_) -> void { self.min_ = min_; }), + +[](AABB& self, const Vec3s& min_) -> void { self.min_ = min_; }), "The min point in the AABB.") .add_property( "max_", bp::make_function( - +[](AABB& self) -> Vec3f& { return self.max_; }, + +[](AABB& self) -> Vec3s& { return self.max_; }, bp::return_internal_reference<>()), bp::make_function( - +[](AABB& self, const Vec3f& max_) -> void { self.max_ = max_; }), + +[](AABB& self, const Vec3s& max_) -> void { self.max_ = max_; }), "The max point in the AABB.") .def(bp::self == bp::self) @@ -604,7 +606,7 @@ void exposeCollisionGeometries() { .def(bp::self + bp::self) .def(bp::self += bp::self) - .def(bp::self += Vec3f()) + .def(bp::self += Vec3s()) .def("size", &AABB::volume, bp::arg("self"), "Size of the AABB.") .def("center", &AABB::center, bp::arg("self"), "Center of the AABB.") @@ -614,24 +616,24 @@ void exposeCollisionGeometries() { .def("volume", &AABB::volume, bp::arg("self"), "Volume of the AABB.") .def("expand", - static_cast(&AABB::expand), + static_cast(&AABB::expand), // doxygen::member_func_doc(static_cast(&AABB::expand)), + // AABB &, CoalScalar)>(&AABB::expand)), // doxygen::member_func_args(static_cast(&AABB::expand)), + // (AABB::*)(const AABB &, CoalScalar)>(&AABB::expand)), bp::return_internal_reference<>()) .def("expand", - static_cast(&AABB::expand), + static_cast(&AABB::expand), // doxygen::member_func_doc(static_cast(&AABB::expand)), + // CoalScalar)>(&AABB::expand)), // doxygen::member_func_args(static_cast(&AABB::expand)), + // (AABB::*)(const CoalScalar)>(&AABB::expand)), bp::return_internal_reference<>()) - .def("expand", static_cast(&AABB::expand), + .def("expand", static_cast(&AABB::expand), // doxygen::member_func_doc(static_cast(&AABB::expand)), + // Vec3s &)>(&AABB::expand)), // doxygen::member_func_args(static_cast(&AABB::expand)), + // (AABB::*)(const Vec3s &)>(&AABB::expand)), bp::return_internal_reference<>()) .def_pickle(PickleObject()) .def(SerializableVisitor()) @@ -640,10 +642,10 @@ void exposeCollisionGeometries() { #endif ; - def("translate", (AABB(*)(const AABB&, const Vec3f&))&translate, + def("translate", (AABB(*)(const AABB&, const Vec3s&))&translate, bp::args("aabb", "t"), "Translate the center of AABB by t"); - def("rotate", (AABB(*)(const AABB&, const Matrix3f&))&rotate, + def("rotate", (AABB(*)(const AABB&, const Matrix3s&))&rotate, bp::args("aabb", "R"), "Rotate the AABB object by R"); if (!eigenpy::register_symbolic_link_to_registered_type< @@ -698,8 +700,7 @@ void exposeCollisionGeometries() { bp::return_internal_reference<>()) .def("vertices", &BVHModelBaseWrapper::vertex, bp::args("self", "index"), "Retrieve the vertex given by its index.", - ::hpp::fcl::python::deprecated_member< - bp::return_internal_reference<>>()) + ::coal::python::deprecated_member>()) .def("vertices", &BVHModelBaseWrapper::vertices, bp::args("self"), "Retrieve all the vertices.", bp::with_custodian_and_ward_postcall<0, 1>()) @@ -725,9 +726,9 @@ void exposeCollisionGeometries() { .def(dv::member_func("addTriangle", &BVHModelBase::addTriangle)) .def(dv::member_func("addTriangles", &BVHModelBase::addTriangles)) .def(dv::member_func("addSubModel", + const Vec3ss&, const Triangles&)>("addSubModel", &BVHModelBase::addSubModel)) - .def(dv::member_func( + .def(dv::member_func( "addSubModel", &BVHModelBase::addSubModel)) .def(dv::member_func("endModel", &BVHModelBase::endModel)) // Expose function to replace a BVH @@ -753,9 +754,9 @@ void exposeCollisionObject() { .def(dv::init>()) .def(dv::init>()) + const Transform3s&, bp::optional>()) .def(dv::init>()) + const Matrix3s&, const Vec3s&, bp::optional>()) .DEF_CLASS_FUNC(CollisionObject, getObjectType) .DEF_CLASS_FUNC(CollisionObject, getNodeType) @@ -775,7 +776,7 @@ void exposeCollisionObject() { bp::return_value_policy()) .def(dv::member_func( "setTransform", - static_cast( + static_cast( &CollisionObject::setTransform))) .DEF_CLASS_FUNC(CollisionObject, isIdentityTransform) diff --git a/python/collision.cc b/python/collision.cc index a759b193a..b50a88190 100644 --- a/python/collision.cc +++ b/python/collision.cc @@ -34,28 +34,28 @@ #include -#include -HPP_FCL_COMPILER_DIAGNOSTIC_PUSH -HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS -#include -#include -HPP_FCL_COMPILER_DIAGNOSTIC_POP +#include "coal/fwd.hh" +COAL_COMPILER_DIAGNOSTIC_PUSH +COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS +#include "coal/collision.h" +#include "coal/serialization/collision_data.h" +COAL_COMPILER_DIAGNOSTIC_POP -#include "fcl.hh" +#include "coal.hh" #include "deprecation.hh" #include "serializable.hh" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" -#include "doxygen_autodoc/hpp/fcl/collision_data.h" +#include "doxygen_autodoc/coal/collision_data.h" #endif #include "../doc/python/doxygen.hh" #include "../doc/python/doxygen-boost.hh" using namespace boost::python; -using namespace hpp::fcl; -using namespace hpp::fcl::python; +using namespace coal; +using namespace coal::python; namespace dv = doxygen::visitor; @@ -65,10 +65,10 @@ const CollisionGeometry* geto(const Contact& c) { } struct ContactWrapper { - static Vec3f getNearestPoint1(const Contact& contact) { + static Vec3s getNearestPoint1(const Contact& contact) { return contact.nearest_points[0]; } - static Vec3f getNearestPoint2(const Contact& contact) { + static Vec3s getNearestPoint2(const Contact& contact) { return contact.nearest_points[1]; } }; @@ -94,8 +94,8 @@ void exposeCollisionAPI() { .def("clear", &CPUTimes::clear, arg("self"), "Reset the time values."); } - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (!eigenpy::register_symbolic_link_to_registered_type()) { class_("QueryRequest", doxygen::class_doc(), no_init) @@ -132,10 +132,10 @@ void exposeCollisionAPI() { .DEF_RW_CLASS_ATTRIB(QueryRequest, enable_timings) .DEF_CLASS_FUNC(QueryRequest, updateGuess); } - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (!eigenpy::register_symbolic_link_to_registered_type()) { class_ >( "CollisionRequest", doxygen::class_doc(), no_init) @@ -174,7 +174,7 @@ void exposeCollisionAPI() { class_ >("StdVec_CollisionRequest") .def(vector_indexing_suite >()); } - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP if (!eigenpy::register_symbolic_link_to_registered_type()) { class_("Contact", doxygen::class_doc(), @@ -182,8 +182,8 @@ void exposeCollisionAPI() { .def(dv::init()) .def(dv::init()) + const CollisionGeometry*, int, int, const Vec3s&, + const Vec3s&, CoalScalar>()) .add_property( "o1", make_function(&geto<1>, @@ -260,8 +260,8 @@ void exposeCollisionAPI() { const CollisionRequest&, CollisionResult&)>(&collide)); doxygen::def( "collide", - static_cast( &collide)); @@ -271,6 +271,6 @@ void exposeCollisionAPI() { const CollisionGeometry*>()) .def("__call__", static_cast(&ComputeCollision::operator())); } diff --git a/python/contact_patch.cc b/python/contact_patch.cc index 66099b3b3..831137b74 100644 --- a/python/contact_patch.cc +++ b/python/contact_patch.cc @@ -34,25 +34,25 @@ #include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/contact_patch.h" +#include "coal/serialization/collision_data.h" -#include "fcl.hh" +#include "coal.hh" #include "deprecation.hh" #include "serializable.hh" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" -#include "doxygen_autodoc/hpp/fcl/collision_data.h" +#include "doxygen_autodoc/coal/collision_data.h" #endif #include "../doc/python/doxygen.hh" #include "../doc/python/doxygen-boost.hh" using namespace boost::python; -using namespace hpp::fcl; -using namespace hpp::fcl::python; +using namespace coal; +using namespace coal::python; namespace dv = doxygen::visitor; @@ -93,12 +93,12 @@ void exposeContactPatchAPI() { ContactPatchRequest>()) { class_( "ContactPatchRequest", doxygen::class_doc(), - init>( + init>( (arg("self"), arg("max_num_patch"), arg("num_samples_curved_shapes"), arg("patch_tolerance")), "ContactPatchRequest constructor.")) .def(dv::init>()) + bp::optional>()) .DEF_RW_CLASS_ATTRIB(ContactPatchRequest, max_num_patch) .DEF_CLASS_FUNC(ContactPatchRequest, getNumSamplesCurvedShapes) .DEF_CLASS_FUNC(ContactPatchRequest, setNumSamplesCurvedShapes) @@ -140,8 +140,8 @@ void exposeContactPatchAPI() { ContactPatchResult&)>(&computeContactPatch)); doxygen::def( "computeContactPatch", - static_cast(&computeContactPatch)); @@ -154,7 +154,7 @@ void exposeContactPatchAPI() { const CollisionGeometry*>()) .def("__call__", static_cast( &ComputeContactPatch::operator())); } diff --git a/python/deprecation.hh b/python/deprecation.hh index 0c5e5adeb..2bc9a20ff 100644 --- a/python/deprecation.hh +++ b/python/deprecation.hh @@ -2,15 +2,14 @@ // Copyright (c) 2020-2021 INRIA // -#ifndef HPP_FCL_PYTHON_UTILS_DEPRECATION_H -#define HPP_FCL_PYTHON_UTILS_DEPRECATION_H +#ifndef COAL_PYTHON_UTILS_DEPRECATION_H +#define COAL_PYTHON_UTILS_DEPRECATION_H #include #include #include -namespace hpp { -namespace fcl { +namespace coal { namespace python { template @@ -48,7 +47,6 @@ struct deprecated_function : deprecated_warning_policy { }; } // namespace python -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_PYTHON_UTILS_DEPRECATION_H +#endif // ifndef COAL_PYTHON_UTILS_DEPRECATION_H diff --git a/python/distance.cc b/python/distance.cc index 0231814da..f4c1a26a6 100644 --- a/python/distance.cc +++ b/python/distance.cc @@ -34,45 +34,46 @@ #include -#include "fcl.hh" +#include "coal.hh" + +COAL_COMPILER_DIAGNOSTIC_PUSH +COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS +#include "coal/fwd.hh" +#include "coal/distance.h" +#include "coal/serialization/collision_data.h" -HPP_FCL_COMPILER_DIAGNOSTIC_PUSH -HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS -#include -#include -#include #include "deprecation.hh" -HPP_FCL_COMPILER_DIAGNOSTIC_POP +COAL_COMPILER_DIAGNOSTIC_POP #include "serializable.hh" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" -#include "doxygen_autodoc/hpp/fcl/collision_data.h" +#include "doxygen_autodoc/coal/collision_data.h" #endif using namespace boost::python; -using namespace hpp::fcl; -using namespace hpp::fcl::python; +using namespace coal; +using namespace coal::python; namespace dv = doxygen::visitor; struct DistanceResultWrapper { - static Vec3f getNearestPoint1(const DistanceResult& res) { + static Vec3s getNearestPoint1(const DistanceResult& res) { return res.nearest_points[0]; } - static Vec3f getNearestPoint2(const DistanceResult& res) { + static Vec3s getNearestPoint2(const DistanceResult& res) { return res.nearest_points[1]; } }; void exposeDistanceAPI() { - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (!eigenpy::register_symbolic_link_to_registered_type()) { class_ >( "DistanceRequest", doxygen::class_doc(), - init >( + init >( (arg("self"), arg("enable_nearest_points"), arg("rel_err"), arg("abs_err")), "Constructor")) @@ -110,7 +111,7 @@ void exposeDistanceAPI() { .DEF_RW_CLASS_ATTRIB(DistanceRequest, abs_err) .def(SerializableVisitor()); } - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP if (!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) { @@ -148,14 +149,14 @@ void exposeDistanceAPI() { doxygen::def( "distance", - static_cast( + static_cast( &distance)); doxygen::def( "distance", - static_cast( + static_cast( &distance)); class_("ComputeDistance", @@ -163,7 +164,7 @@ void exposeDistanceAPI() { .def(dv::init()) .def("__call__", - static_cast(&ComputeDistance::operator())); } diff --git a/python/fwd.hh b/python/fwd.hh index 45e24bcbf..62a197e19 100644 --- a/python/fwd.hh +++ b/python/fwd.hh @@ -2,13 +2,13 @@ // Copyright (c) 2022 CNRS INRIA // -#ifndef HPP_FCL_PYTHON_FWD_HH -#define HPP_FCL_PYTHON_FWD_HH +#ifndef COAL_PYTHON_FWD_HH +#define COAL_PYTHON_FWD_HH -#include -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#include "coal/fwd.hh" +#ifdef COAL_HAS_DOXYGEN_AUTODOC namespace doxygen { -using hpp::fcl::shared_ptr; +using coal::shared_ptr; } #endif @@ -37,4 +37,4 @@ namespace dv = doxygen::visitor; #define DEF_CLASS_FUNC2(CLASS, ATTRIB, policy) \ def(#ATTRIB, &CLASS::ATTRIB, doxygen::member_func_doc(&CLASS::ATTRIB), policy) -#endif // ifndef HPP_FCL_PYTHON_FWD_HH +#endif // ifndef COAL_PYTHON_FWD_HH diff --git a/python/gjk.cc b/python/gjk.cc index c88e9b826..cab5ebde1 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -34,25 +34,25 @@ #include -#include "fcl.hh" +#include "coal.hh" -#include -#include +#include "coal/fwd.hh" +#include "coal/narrowphase/gjk.h" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" -#include "doxygen_autodoc/hpp/fcl/narrowphase/gjk.h" +#include "doxygen_autodoc/coal/narrowphase/gjk.h" #endif using namespace boost::python; -using namespace hpp::fcl; -using hpp::fcl::details::EPA; -using hpp::fcl::details::GJK; -using hpp::fcl::details::MinkowskiDiff; -using hpp::fcl::details::SupportOptions; +using namespace coal; +using coal::details::EPA; +using coal::details::GJK; +using coal::details::MinkowskiDiff; +using coal::details::SupportOptions; struct MinkowskiDiffWrapper { - static void support0(MinkowskiDiff& self, const Vec3f& dir, int& hint, + static void support0(MinkowskiDiff& self, const Vec3s& dir, int& hint, bool compute_swept_sphere_support = false) { if (compute_swept_sphere_support) { self.support0(dir, hint); @@ -61,7 +61,7 @@ struct MinkowskiDiffWrapper { } } - static void support1(MinkowskiDiff& self, const Vec3f& dir, int& hint, + static void support1(MinkowskiDiff& self, const Vec3s& dir, int& hint, bool compute_swept_sphere_support = false) { if (compute_swept_sphere_support) { self.support1(dir, hint); @@ -81,8 +81,8 @@ struct MinkowskiDiffWrapper { } static void set(MinkowskiDiff& self, const ShapeBase* shape0, - const ShapeBase* shape1, const Transform3f& tf0, - const Transform3f& tf1, + const ShapeBase* shape1, const Transform3s& tf0, + const Transform3s& tf1, bool compute_swept_sphere_supports = false) { if (compute_swept_sphere_supports) { self.set(shape0, shape1, tf0, tf1); @@ -119,13 +119,13 @@ void exposeGJK() { .def("set", static_cast( + const ShapeBase*, const Transform3s&, + const Transform3s&, bool)>( &MinkowskiDiffWrapper::set), doxygen::member_func_doc( static_cast( + const ShapeBase*, const ShapeBase*, const Transform3s&, + const Transform3s&)>( &MinkowskiDiff::set))) .def("support0", &MinkowskiDiffWrapper::support0, @@ -178,7 +178,7 @@ void exposeGJK() { if (!eigenpy::register_symbolic_link_to_registered_type()) { class_("GJK", doxygen::class_doc(), no_init) - .def(doxygen::visitor::init()) + .def(doxygen::visitor::init()) .DEF_RW_CLASS_ATTRIB(GJK, distance) .DEF_RW_CLASS_ATTRIB(GJK, ray) .DEF_RW_CLASS_ATTRIB(GJK, support_hint) diff --git a/python/hppfcl/__init__.py b/python/hppfcl/__init__.py index b93784303..1948a5878 100644 --- a/python/hppfcl/__init__.py +++ b/python/hppfcl/__init__.py @@ -1,36 +1,9 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2019 INRIA -# Author: Justin Carpentier -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of CNRS-LAAS. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. +import warnings -# ruff: noqa: F401, F403 -from .hppfcl import * -from .hppfcl import __raw_version__, __version__ +warnings.warn( + "Please update your 'hppfcl' imports to 'coal'", category=DeprecationWarning +) + +from coal import Transform3s as Transform3f # noqa +from coal import * # noqa +from coal import __raw_version__, __version__ # noqa diff --git a/python/hppfcl/viewer.py b/python/hppfcl/viewer.py index 15bf53c2d..13353dce9 100644 --- a/python/hppfcl/viewer.py +++ b/python/hppfcl/viewer.py @@ -1,110 +1,7 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2019 CNRS -# Author: Joseph Mirabel - import warnings -import numpy as np -from gepetto import Color - -import hppfcl - - -def applyConfiguration(gui, name, tf): - gui.applyConfiguration( - name, tf.getTranslation().tolist() + tf.getQuatRotation().coeffs().tolist() - ) - - -def displayShape(gui, name, geom, color=(0.9, 0.9, 0.9, 1.0)): - if isinstance(geom, hppfcl.Capsule): - return gui.addCapsule(name, geom.radius, 2.0 * geom.halfLength, color) - elif isinstance(geom, hppfcl.Cylinder): - return gui.addCylinder(name, geom.radius, 2.0 * geom.halfLength, color) - elif isinstance(geom, hppfcl.Box): - w, h, d = (2.0 * geom.halfSide).tolist() - return gui.addBox(name, w, h, d, color) - elif isinstance(geom, hppfcl.Sphere): - return gui.addSphere(name, geom.radius, color) - elif isinstance(geom, hppfcl.Cone): - return gui.addCone(name, geom.radius, 2.0 * geom.halfLength, color) - elif isinstance(geom, hppfcl.Convex): - pts = [ - geom.points(geom.polygons(f)[i]).tolist() - for f in range(geom.num_polygons) - for i in range(3) - ] - gui.addCurve(name, pts, color) - gui.setCurveMode(name, "TRIANGLES") - gui.setLightingMode(name, "ON") - gui.setBoolProperty(name, "BackfaceDrawing", True) - return True - elif isinstance(geom, hppfcl.ConvexBase): - pts = [geom.points(i).tolist() for i in range(geom.num_points)] - gui.addCurve(name, pts, color) - gui.setCurveMode(name, "POINTS") - gui.setLightingMode(name, "OFF") - return True - else: - msg = "Unsupported geometry type for %s (%s)" % (name, type(geom)) - warnings.warn(msg, category=UserWarning, stacklevel=2) - return False - - -def displayDistanceResult(gui, group_name, res, closest_points=True, normal=True): - gui.createGroup(group_name) - r = 0.01 - if closest_points: - p = [group_name + "/p1", group_name + "/p2"] - gui.addSphere(p[0], r, Color.red) - gui.addSphere(p[1], r, Color.blue) - qid = [0, 0, 0, 1] - gui.applyConfigurations( - p, - [ - res.getNearestPoint1().tolist() + qid, - res.getNearestPoint2().tolist() + qid, - ], - ) - if normal: - n = group_name + "/normal" - gui.addArrow(n, r, 0.1, Color.green) - gui.applyConfiguration( - n, - res.getNearestPoint1().tolist() - + hppfcl.Quaternion.FromTwoVectors(np.array([1, 0, 0]), res.normal) - .coeffs() - .tolist(), - ) - gui.refresh() - +warnings.warn( + "Please update your 'hppfcl' imports to 'coal'", category=DeprecationWarning +) -def displayCollisionResult(gui, group_name, res, color=Color.green): - if res.isCollision(): - if gui.nodeExists(group_name): - gui.setVisibility(group_name, "ON") - else: - gui.createGroup(group_name) - for i in range(res.numContacts()): - contact = res.getContact(i) - n = group_name + "/contact" + str(i) - depth = contact.penetration_depth - if gui.nodeExists(n): - gui.setFloatProperty(n, "Size", depth) - gui.setFloatProperty(n, "Radius", 0.1 * depth) - gui.setColor(n, color) - else: - gui.addArrow(n, depth * 0.1, depth, color) - N = contact.normal - P = contact.pos - gui.applyConfiguration( - n, - (P - depth * N / 2).tolist() - + hppfcl.Quaternion.FromTwoVectors(np.array([1, 0, 0]), N) - .coeffs() - .tolist(), - ) - gui.refresh() - elif gui.nodeExists(group_name): - gui.setVisibility(group_name, "OFF") +from coal.viewer import * # noqa diff --git a/python/math.cc b/python/math.cc index 30c20d4ce..0261ae5f1 100644 --- a/python/math.cc +++ b/python/math.cc @@ -35,21 +35,21 @@ #include #include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/math/transform.h" +#include "coal/serialization/transform.h" -#include "fcl.hh" +#include "coal.hh" #include "pickle.hh" #include "serializable.hh" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC -#include "doxygen_autodoc/hpp/fcl/math/transform.h" +#ifdef COAL_HAS_DOXYGEN_AUTODOC +#include "doxygen_autodoc/coal/math/transform.h" #endif using namespace boost::python; -using namespace hpp::fcl; -using namespace hpp::fcl::python; +using namespace coal; +using namespace coal::python; namespace dv = doxygen::visitor; @@ -57,12 +57,12 @@ struct TriangleWrapper { static Triangle::index_type getitem(const Triangle& t, int i) { if (i >= 3 || i <= -3) PyErr_SetString(PyExc_IndexError, "Index out of range"); - return t[static_cast(i % 3)]; + return t[static_cast(i % 3)]; } static void setitem(Triangle& t, int i, Triangle::index_type v) { if (i >= 3 || i <= -3) PyErr_SetString(PyExc_IndexError, "Index out of range"); - t[static_cast(i % 3)] = v; + t[static_cast(i % 3)] = v; } }; @@ -74,56 +74,60 @@ void exposeMaths() { if (!eigenpy::register_symbolic_link_to_registered_type()) eigenpy::exposeAngleAxis(); - eigenpy::enableEigenPySpecific(); - eigenpy::enableEigenPySpecific(); - - class_("Transform3f", doxygen::class_doc(), no_init) - .def(dv::init()) - .def(dv::init()) - .def(dv::init()) - .def(dv::init()) - .def(dv::init()) - .def(dv::init()) - .def(dv::init()) - - .def(dv::member_func("getQuatRotation", &Transform3f::getQuatRotation)) - .def("getTranslation", &Transform3f::getTranslation, - doxygen::member_func_doc(&Transform3f::getTranslation), + eigenpy::enableEigenPySpecific(); + eigenpy::enableEigenPySpecific(); + + class_("Transform3s", doxygen::class_doc(), no_init) + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + + .def(dv::member_func("getQuatRotation", &Transform3s::getQuatRotation)) + .def("getTranslation", &Transform3s::getTranslation, + doxygen::member_func_doc(&Transform3s::getTranslation), return_value_policy()) - .def("getRotation", &Transform3f::getRotation, + .def("getRotation", &Transform3s::getRotation, return_value_policy()) - .def("isIdentity", &Transform3f::isIdentity, + .def("isIdentity", &Transform3s::isIdentity, (bp::arg("self"), - bp::arg("prec") = Eigen::NumTraits::dummy_precision()), - doxygen::member_func_doc(&Transform3f::getTranslation)) + bp::arg("prec") = Eigen::NumTraits::dummy_precision()), + doxygen::member_func_doc(&Transform3s::getTranslation)) - .def(dv::member_func("setQuatRotation", &Transform3f::setQuatRotation)) - .def("setTranslation", &Transform3f::setTranslation) - .def("setRotation", &Transform3f::setRotation) + .def(dv::member_func("setQuatRotation", &Transform3s::setQuatRotation)) + .def("setTranslation", &Transform3s::setTranslation) + .def("setRotation", &Transform3s::setRotation) .def(dv::member_func("setTransform", - &Transform3f::setTransform)) + &Transform3s::setTransform)) .def(dv::member_func( "setTransform", - static_cast( - &Transform3f::setTransform))) - .def(dv::member_func("setIdentity", &Transform3f::setIdentity)) - .def(dv::member_func("Identity", &Transform3f::Identity)) + static_cast( + &Transform3s::setTransform))) + .def(dv::member_func("setIdentity", &Transform3s::setIdentity)) + .def(dv::member_func("Identity", &Transform3s::Identity)) .staticmethod("Identity") - .def(dv::member_func("transform", &Transform3f::transform)) - .def("inverseInPlace", &Transform3f::inverseInPlace, + .def(dv::member_func("setRandom", &Transform3s::setRandom)) + .def(dv::member_func("Random", &Transform3s::Random)) + .staticmethod("Random") + + .def(dv::member_func("transform", &Transform3s::transform)) + .def("inverseInPlace", &Transform3s::inverseInPlace, return_internal_reference<>(), - doxygen::member_func_doc(&Transform3f::inverseInPlace)) - .def(dv::member_func("inverse", &Transform3f::inverse)) - .def(dv::member_func("inverseTimes", &Transform3f::inverseTimes)) + doxygen::member_func_doc(&Transform3s::inverseInPlace)) + .def(dv::member_func("inverse", &Transform3s::inverse)) + .def(dv::member_func("inverseTimes", &Transform3s::inverseTimes)) .def(self * self) .def(self *= self) .def(self == self) .def(self != self) - .def_pickle(PickleObject()) - .def(SerializableVisitor()); + .def_pickle(PickleObject()) + .def(SerializableVisitor()); class_("Triangle", no_init) .def(dv::init()) @@ -137,9 +141,9 @@ void exposeMaths() { .def(self == self); if (!eigenpy::register_symbolic_link_to_registered_type< - std::vector >()) { - class_ >("StdVec_Vec3f") - .def(vector_indexing_suite >()); + std::vector >()) { + class_ >("StdVec_Vec3s") + .def(vector_indexing_suite >()); } if (!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) { diff --git a/python/octree.cc b/python/octree.cc index 76ebc8b7f..a33ac1665 100644 --- a/python/octree.cc +++ b/python/octree.cc @@ -1,14 +1,14 @@ -#include "fcl.hh" +#include "coal.hh" #include -#include -#include +#include "coal/fwd.hh" +#include "coal/octree.h" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" -#include "doxygen_autodoc/hpp/fcl/octree.h" +#include "doxygen_autodoc/coal/octree.h" #endif bp::object toPyBytes(std::vector& bytes) { @@ -23,19 +23,19 @@ bp::object toPyBytes(std::vector& bytes) { #endif } -bp::object tobytes(const hpp::fcl::OcTree& self) { +bp::object tobytes(const coal::OcTree& self) { std::vector bytes = self.tobytes(); return toPyBytes(bytes); } void exposeOctree() { - using namespace hpp::fcl; + using namespace coal; namespace bp = boost::python; namespace dv = doxygen::visitor; bp::class_, shared_ptr >( "OcTree", doxygen::class_doc(), bp::no_init) - .def(dv::init()) + .def(dv::init()) .def("clone", &OcTree::clone, doxygen::member_func_doc(&OcTree::clone), bp::return_value_policy()) .def(dv::member_func("getTreeDepth", &OcTree::getTreeDepth)) @@ -53,7 +53,7 @@ void exposeOctree() { .def("tobytes", tobytes, doxygen::member_func_doc(&OcTree::tobytes)); doxygen::def("makeOctree", &makeOctree); - eigenpy::enableEigenPySpecific(); - eigenpy::StdVectorPythonVisitor, true>::expose( + eigenpy::enableEigenPySpecific(); + eigenpy::StdVectorPythonVisitor, true>::expose( "StdVec_Vec6"); } diff --git a/python/pickle.hh b/python/pickle.hh index 3e640cf0d..7e2358582 100644 --- a/python/pickle.hh +++ b/python/pickle.hh @@ -2,8 +2,8 @@ // Copyright (c) 2022 INRIA // -#ifndef HPP_FCL_PYTHON_PICKLE_H -#define HPP_FCL_PYTHON_PICKLE_H +#ifndef COAL_PYTHON_PICKLE_H +#define COAL_PYTHON_PICKLE_H #include #include @@ -13,7 +13,7 @@ #include using namespace boost::python; -using namespace hpp::fcl; +using namespace coal; // template struct PickleObject : boost::python::pickle_suite { @@ -54,4 +54,4 @@ struct PickleObject : boost::python::pickle_suite { static bool getstate_manages_dict() { return false; } }; -#endif // ifndef HPP_FCL_PYTHON_PICKLE_H +#endif // ifndef COAL_PYTHON_PICKLE_H diff --git a/python/serializable.hh b/python/serializable.hh index 36950f9f6..2ab337fb5 100644 --- a/python/serializable.hh +++ b/python/serializable.hh @@ -4,19 +4,18 @@ // https://github.com/stack-of-tasks/pinocchio // -#ifndef HPP_FCL_PYTHON_SERIALIZABLE_H -#define HPP_FCL_PYTHON_SERIALIZABLE_H +#ifndef COAL_PYTHON_SERIALIZABLE_H +#define COAL_PYTHON_SERIALIZABLE_H #include -#include "hpp/fcl/serialization/archive.h" -#include "hpp/fcl/serialization/serializer.h" +#include "coal/serialization/archive.h" +#include "coal/serialization/serializer.h" -namespace hpp { -namespace fcl { +namespace coal { namespace python { -using Serializer = ::hpp::fcl::serialization::Serializer; +using Serializer = ::coal::serialization::Serializer; namespace bp = boost::python; @@ -54,7 +53,6 @@ struct SerializableVisitor } }; } // namespace python -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_PYTHON_SERIALIZABLE_H +#endif // ifndef COAL_PYTHON_SERIALIZABLE_H diff --git a/python/utils/std-pair.hh b/python/utils/std-pair.hh index 809467201..8af3520a0 100644 --- a/python/utils/std-pair.hh +++ b/python/utils/std-pair.hh @@ -2,8 +2,8 @@ // Copyright (c) 2023 INRIA // -#ifndef HPP_FCL_PYTHON_UTILS_STD_PAIR_H -#define HPP_FCL_PYTHON_UTILS_STD_PAIR_H +#ifndef COAL_PYTHON_UTILS_STD_PAIR_H +#define COAL_PYTHON_UTILS_STD_PAIR_H #include #include @@ -62,4 +62,4 @@ struct StdPairConverter { } }; -#endif // ifndef HPP_FCL_PYTHON_UTILS_STD_PAIR_H +#endif // ifndef COAL_PYTHON_UTILS_STD_PAIR_H diff --git a/python/version.cc b/python/version.cc index 4bc0eea93..658a94297 100644 --- a/python/version.cc +++ b/python/version.cc @@ -2,36 +2,36 @@ // Copyright (c) 2019-2023 CNRS INRIA // -#include "hpp/fcl/config.hh" -#include "fcl.hh" +#include "coal/config.hh" +#include "coal.hh" #include namespace bp = boost::python; inline bool checkVersionAtLeast(int major, int minor, int patch) { - return HPP_FCL_VERSION_AT_LEAST(major, minor, patch); + return COAL_VERSION_AT_LEAST(major, minor, patch); } inline bool checkVersionAtMost(int major, int minor, int patch) { - return HPP_FCL_VERSION_AT_MOST(major, minor, patch); + return COAL_VERSION_AT_MOST(major, minor, patch); } void exposeVersion() { - // Define release numbers of the current hpp-fcl version. + // Define release numbers of the current Coal version. bp::scope().attr("__version__") = - BOOST_PP_STRINGIZE(HPP_FCL_MAJOR_VERSION) "." BOOST_PP_STRINGIZE(HPP_FCL_MINOR_VERSION) "." BOOST_PP_STRINGIZE(HPP_FCL_PATCH_VERSION); - bp::scope().attr("__raw_version__") = HPP_FCL_VERSION; - bp::scope().attr("HPP_FCL_MAJOR_VERSION") = HPP_FCL_MAJOR_VERSION; - bp::scope().attr("HPP_FCL_MINOR_VERSION") = HPP_FCL_MINOR_VERSION; - bp::scope().attr("HPP_FCL_PATCH_VERSION") = HPP_FCL_PATCH_VERSION; + BOOST_PP_STRINGIZE(COAL_MAJOR_VERSION) "." BOOST_PP_STRINGIZE(COAL_MINOR_VERSION) "." BOOST_PP_STRINGIZE(COAL_PATCH_VERSION); + bp::scope().attr("__raw_version__") = COAL_VERSION; + bp::scope().attr("COAL_MAJOR_VERSION") = COAL_MAJOR_VERSION; + bp::scope().attr("COAL_MINOR_VERSION") = COAL_MINOR_VERSION; + bp::scope().attr("COAL_PATCH_VERSION") = COAL_PATCH_VERSION; -#if HPP_FCL_HAS_QHULL +#if COAL_HAS_QHULL bp::scope().attr("WITH_QHULL") = true; #else bp::scope().attr("WITH_QHULL") = false; #endif -#if HPP_FCL_HAS_OCTOMAP +#if COAL_HAS_OCTOMAP bp::scope().attr("WITH_OCTOMAP") = true; #else bp::scope().attr("WITH_OCTOMAP") = false; @@ -39,11 +39,11 @@ void exposeVersion() { bp::def("checkVersionAtLeast", &checkVersionAtLeast, bp::args("major", "minor", "patch"), - "Checks if the current version of hpp-fcl is at least" + "Checks if the current version of coal is at least" " the version provided by the input arguments."); bp::def("checkVersionAtMost", &checkVersionAtMost, bp::args("major", "minor", "patch"), - "Checks if the current version of hpp-fcl is at most" + "Checks if the current version of coal is at most" " the version provided by the input arguments."); } diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp index 97f1b0723..4e28a2548 100644 --- a/src/BV/AABB.cpp +++ b/src/BV/AABB.cpp @@ -35,36 +35,35 @@ /** \author Jia Pan */ -#include -#include +#include "coal/BV/AABB.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/collision_data.h" #include -#include -namespace hpp { -namespace fcl { +namespace coal { AABB::AABB() - : min_(Vec3f::Constant((std::numeric_limits::max)())), - max_(Vec3f::Constant(-(std::numeric_limits::max)())) {} + : min_(Vec3s::Constant((std::numeric_limits::max)())), + max_(Vec3s::Constant(-(std::numeric_limits::max)())) {} bool AABB::overlap(const AABB& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const { - const FCL_REAL break_distance_squared = + CoalScalar& sqrDistLowerBound) const { + const CoalScalar break_distance_squared = request.break_distance * request.break_distance; sqrDistLowerBound = - (min_ - other.max_ - Vec3f::Constant(request.security_margin)) + (min_ - other.max_ - Vec3s::Constant(request.security_margin)) .array() - .max(FCL_REAL(0)) + .max(CoalScalar(0)) .matrix() .squaredNorm(); if (sqrDistLowerBound > break_distance_squared) return false; sqrDistLowerBound = - (other.min_ - max_ - Vec3f::Constant(request.security_margin)) + (other.min_ - max_ - Vec3s::Constant(request.security_margin)) .array() - .max(FCL_REAL(0)) + .max(CoalScalar(0)) .matrix() .squaredNorm(); if (sqrDistLowerBound > break_distance_squared) return false; @@ -72,23 +71,23 @@ bool AABB::overlap(const AABB& other, const CollisionRequest& request, return true; } -FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const { - FCL_REAL result = 0; +CoalScalar AABB::distance(const AABB& other, Vec3s* P, Vec3s* Q) const { + CoalScalar result = 0; for (Eigen::DenseIndex i = 0; i < 3; ++i) { - const FCL_REAL& amin = min_[i]; - const FCL_REAL& amax = max_[i]; - const FCL_REAL& bmin = other.min_[i]; - const FCL_REAL& bmax = other.max_[i]; + const CoalScalar& amin = min_[i]; + const CoalScalar& amax = max_[i]; + const CoalScalar& bmin = other.min_[i]; + const CoalScalar& bmax = other.max_[i]; if (amin > bmax) { - FCL_REAL delta = bmax - amin; + CoalScalar delta = bmax - amin; result += delta * delta; if (P && Q) { (*P)[i] = amin; (*Q)[i] = bmax; } } else if (bmin > amax) { - FCL_REAL delta = amax - bmin; + CoalScalar delta = amax - bmin; result += delta * delta; if (P && Q) { (*P)[i] = amax; @@ -97,11 +96,11 @@ FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const { } else { if (P && Q) { if (bmin >= amin) { - FCL_REAL t = 0.5 * (amax + bmin); + CoalScalar t = 0.5 * (amax + bmin); (*P)[i] = t; (*Q)[i] = t; } else { - FCL_REAL t = 0.5 * (amin + bmax); + CoalScalar t = 0.5 * (amin + bmax); (*P)[i] = t; (*Q)[i] = t; } @@ -112,19 +111,19 @@ FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const { return std::sqrt(result); } -FCL_REAL AABB::distance(const AABB& other) const { - FCL_REAL result = 0; +CoalScalar AABB::distance(const AABB& other) const { + CoalScalar result = 0; for (Eigen::DenseIndex i = 0; i < 3; ++i) { - const FCL_REAL& amin = min_[i]; - const FCL_REAL& amax = max_[i]; - const FCL_REAL& bmin = other.min_[i]; - const FCL_REAL& bmax = other.max_[i]; + const CoalScalar& amin = min_[i]; + const CoalScalar& amax = max_[i]; + const CoalScalar& bmin = other.min_[i]; + const CoalScalar& bmax = other.max_[i]; if (amin > bmax) { - FCL_REAL delta = bmax - amin; + CoalScalar delta = bmax - amin; result += delta * delta; } else if (bmin > amax) { - FCL_REAL delta = amax - bmin; + CoalScalar delta = amax - bmin; result += delta * delta; } } @@ -132,15 +131,15 @@ FCL_REAL AABB::distance(const AABB& other) const { return std::sqrt(result); } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, +bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1, const AABB& b2) { AABB bb1(translate(rotate(b1, R0), T0)); return bb1.overlap(b2); } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, +bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1, const AABB& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) { + CoalScalar& sqrDistLowerBound) { AABB bb1(translate(rotate(b1, R0), T0)); return bb1.overlap(b2, request, sqrDistLowerBound); } @@ -150,15 +149,15 @@ bool AABB::overlap(const Plane& p) const { // points in the directions normal and -normal. // If both points lie on different sides of the plane, there is an overlap // between the AABB and the plane. Otherwise, there is no overlap. - const Vec3f halfside = (this->max_ - this->min_) / 2; - const Vec3f center = (this->max_ + this->min_) / 2; + const Vec3s halfside = (this->max_ - this->min_) / 2; + const Vec3s center = (this->max_ + this->min_) / 2; - const Vec3f support1 = (p.n.array() > 0).select(halfside, -halfside) + center; - const Vec3f support2 = + const Vec3s support1 = (p.n.array() > 0).select(halfside, -halfside) + center; + const Vec3s support2 = ((-p.n).array() > 0).select(halfside, -halfside) + center; - const FCL_REAL dist1 = p.n.dot(support1) - p.d; - const FCL_REAL dist2 = p.n.dot(support2) - p.d; + const CoalScalar dist1 = p.n.dot(support1) - p.d; + const CoalScalar dist2 = p.n.dot(support2) - p.d; const int sign1 = (dist1 > 0) ? 1 : -1; const int sign2 = (dist2 > 0) ? 1 : -1; @@ -170,8 +169,8 @@ bool AABB::overlap(const Plane& p) const { // Both supports are on the same side of the plane. // We now need to check if they are on the same side of the plane inflated // by the swept-sphere radius. - const FCL_REAL ssr_dist1 = std::abs(dist1) - p.getSweptSphereRadius(); - const FCL_REAL ssr_dist2 = std::abs(dist2) - p.getSweptSphereRadius(); + const CoalScalar ssr_dist1 = std::abs(dist1) - p.getSweptSphereRadius(); + const CoalScalar ssr_dist2 = std::abs(dist2) - p.getSweptSphereRadius(); const int ssr_sign1 = (ssr_dist1 > 0) ? 1 : -1; const int ssr_sign2 = (ssr_dist2 > 0) ? 1 : -1; return ssr_sign1 != ssr_sign2; @@ -186,12 +185,10 @@ bool AABB::overlap(const Halfspace& hs) const { // If the support is below the plane defined by the halfspace, there is an // overlap between the AABB and the halfspace. Otherwise, there is no // overlap. - Vec3f halfside = (this->max_ - this->min_) / 2; - Vec3f center = (this->max_ + this->min_) / 2; - Vec3f support = ((-hs.n).array() > 0).select(halfside, -halfside) + center; + Vec3s halfside = (this->max_ - this->min_) / 2; + Vec3s center = (this->max_ + this->min_) / 2; + Vec3s support = ((-hs.n).array() > 0).select(halfside, -halfside) + center; return (hs.signedDistance(support) < 0); } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp index 892d25bd8..b956fe144 100644 --- a/src/BV/OBB.cpp +++ b/src/BV/OBB.cpp @@ -35,45 +35,44 @@ /** \author Jia Pan, Florent Lamiraux */ -#include -#include -#include -#include -#include +#include "coal/BV/OBB.h" +#include "coal/BVH/BVH_utility.h" +#include "coal/math/transform.h" +#include "coal/collision_data.h" +#include "coal/internal/tools.h" #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief Compute the 8 vertices of a OBB -inline void computeVertices(const OBB& b, Vec3f vertices[8]) { - Matrix3f extAxes(b.axes * b.extent.asDiagonal()); - vertices[0].noalias() = b.To + extAxes * Vec3f(-1, -1, -1); - vertices[1].noalias() = b.To + extAxes * Vec3f(1, -1, -1); - vertices[2].noalias() = b.To + extAxes * Vec3f(1, 1, -1); - vertices[3].noalias() = b.To + extAxes * Vec3f(-1, 1, -1); - vertices[4].noalias() = b.To + extAxes * Vec3f(-1, -1, 1); - vertices[5].noalias() = b.To + extAxes * Vec3f(1, -1, 1); - vertices[6].noalias() = b.To + extAxes * Vec3f(1, 1, 1); - vertices[7].noalias() = b.To + extAxes * Vec3f(-1, 1, 1); +inline void computeVertices(const OBB& b, Vec3s vertices[8]) { + Matrix3s extAxes(b.axes * b.extent.asDiagonal()); + vertices[0].noalias() = b.To + extAxes * Vec3s(-1, -1, -1); + vertices[1].noalias() = b.To + extAxes * Vec3s(1, -1, -1); + vertices[2].noalias() = b.To + extAxes * Vec3s(1, 1, -1); + vertices[3].noalias() = b.To + extAxes * Vec3s(-1, 1, -1); + vertices[4].noalias() = b.To + extAxes * Vec3s(-1, -1, 1); + vertices[5].noalias() = b.To + extAxes * Vec3s(1, -1, 1); + vertices[6].noalias() = b.To + extAxes * Vec3s(1, 1, 1); + vertices[7].noalias() = b.To + extAxes * Vec3s(-1, 1, 1); } /// @brief OBB merge method when the centers of two smaller OBB are far away inline OBB merge_largedist(const OBB& b1, const OBB& b2) { OBB b; - Vec3f vertex[16]; + Vec3s vertex[16]; computeVertices(b1, vertex); computeVertices(b2, vertex + 8); - Matrix3f M; - Vec3f E[3]; - Matrix3f::Scalar s[3] = {0, 0, 0}; + Matrix3s M; + Vec3s E[3]; + CoalScalar s[3] = {0, 0, 0}; // obb axes b.axes.col(0).noalias() = (b1.To - b2.To).normalized(); - Vec3f vertex_proj[16]; + Vec3s vertex_proj[16]; for (int i = 0; i < 16; ++i) vertex_proj[i].noalias() = vertex[i] - b.axes.col(0) * vertex[i].dot(b.axes.col(0)); @@ -103,7 +102,7 @@ inline OBB merge_largedist(const OBB& b1, const OBB& b2) { b.axes.col(2) << E[0][mid], E[1][mid], E[2][mid]; // set obb centers and extensions - Vec3f center, extent; + Vec3s center, extent; getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axes, center, extent); b.To.noalias() = center; @@ -122,16 +121,16 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2) { Quatf q((q0.coeffs() + q1.coeffs()).normalized()); b.axes = q.toRotationMatrix(); - Vec3f vertex[8], diff; - FCL_REAL real_max = (std::numeric_limits::max)(); - Vec3f pmin(real_max, real_max, real_max); - Vec3f pmax(-real_max, -real_max, -real_max); + Vec3s vertex[8], diff; + CoalScalar real_max = (std::numeric_limits::max)(); + Vec3s pmin(real_max, real_max, real_max); + Vec3s pmax(-real_max, -real_max, -real_max); computeVertices(b1, vertex); for (int i = 0; i < 8; ++i) { diff = vertex[i] - b.To; for (int j = 0; j < 3; ++j) { - FCL_REAL dot = diff.dot(b.axes.col(j)); + CoalScalar dot = diff.dot(b.axes.col(j)); if (dot > pmax[j]) pmax[j] = dot; else if (dot < pmin[j]) @@ -143,7 +142,7 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2) { for (int i = 0; i < 8; ++i) { diff = vertex[i] - b.To; for (int j = 0; j < 3; ++j) { - FCL_REAL dot = diff.dot(b.axes.col(j)); + CoalScalar dot = diff.dot(b.axes.col(j)); if (dot > pmax[j]) pmax[j] = dot; else if (dot < pmin[j]) @@ -159,12 +158,12 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2) { return b; } -bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b) { - FCL_REAL t, s; - const FCL_REAL reps = 1e-6; +bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b) { + CoalScalar t, s; + const CoalScalar reps = 1e-6; - Matrix3f Bf(B.array().abs() + reps); + Matrix3s Bf(B.array().abs() + reps); // Bf += reps; // if any of these tests are one-sided, then the polyhedra are disjoint @@ -287,20 +286,20 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, } namespace internal { -inline FCL_REAL obbDisjoint_check_A_axis(const Vec3f& T, const Vec3f& a, - const Vec3f& b, const Matrix3f& Bf) { +inline CoalScalar obbDisjoint_check_A_axis(const Vec3s& T, const Vec3s& a, + const Vec3s& b, const Matrix3s& Bf) { // |T| - |B| * b - a - Vec3f AABB_corner(T.cwiseAbs() - a); + Vec3s AABB_corner(T.cwiseAbs() - a); AABB_corner.noalias() -= Bf * b; - return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm(); + return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm(); } -inline FCL_REAL obbDisjoint_check_B_axis(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, - const Matrix3f& Bf) { +inline CoalScalar obbDisjoint_check_B_axis(const Matrix3s& B, const Vec3s& T, + const Vec3s& a, const Vec3s& b, + const Matrix3s& Bf) { // Bf = |B| // | B^T T| - Bf^T * a - b - FCL_REAL s, t = 0; + CoalScalar s, t = 0; s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0]; if (s > 0) t += s * s; s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1]; @@ -311,17 +310,17 @@ inline FCL_REAL obbDisjoint_check_B_axis(const Matrix3f& B, const Vec3f& T, } template -struct HPP_FCL_LOCAL obbDisjoint_check_Ai_cross_Bi{static inline bool run( - int ia, int ja, int ka, const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b, const Matrix3f& Bf, const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance){FCL_REAL sinus2 = - 1 - Bf(ia, ib) * Bf(ia, ib); +struct COAL_LOCAL obbDisjoint_check_Ai_cross_Bi{static inline bool run( + int ia, int ja, int ka, const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b, const Matrix3s& Bf, const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance){CoalScalar sinus2 = + 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 < 1e-6) return false; -const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); +const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); -const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + - b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); +const CoalScalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); // We need to divide by the norm || Aia x Bib || // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 if (diff > 0) { @@ -332,8 +331,8 @@ if (diff > 0) { } return false; } // namespace internal -}; // namespace fcl -} // namespace hpp +}; // namespace coal +} // namespace internal // B, T orientation and position of 2nd OBB in frame of 1st OBB, // a extent of 1st OBB, @@ -341,26 +340,26 @@ return false; // // This function tests whether bounding boxes should be broken down. // -bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T, - const Vec3f& a_, const Vec3f& b_, +bool obbDisjointAndLowerBoundDistance(const Matrix3s& B, const Vec3s& T, + const Vec3s& a_, const Vec3s& b_, const CollisionRequest& request, - FCL_REAL& squaredLowerBoundDistance) { + CoalScalar& squaredLowerBoundDistance) { assert(request.security_margin > -2 * (std::min)(a_.minCoeff(), b_.minCoeff()) - - 10 * Eigen::NumTraits::epsilon() && + 10 * Eigen::NumTraits::epsilon() && "A negative security margin could not be lower than the OBB extent."); - // const FCL_REAL breakDistance(request.break_distance + + // const CoalScalar breakDistance(request.break_distance + // request.security_margin); - const FCL_REAL breakDistance2 = + const CoalScalar breakDistance2 = request.break_distance * request.break_distance; - Matrix3f Bf(B.cwiseAbs()); - const Vec3f a((a_ + Vec3f::Constant(request.security_margin / 2)) + Matrix3s Bf(B.cwiseAbs()); + const Vec3s a((a_ + Vec3s::Constant(request.security_margin / 2)) .array() - .max(FCL_REAL(0))); - const Vec3f b((b_ + Vec3f::Constant(request.security_margin / 2)) + .max(CoalScalar(0))); + const Vec3s b((b_ + Vec3s::Constant(request.security_margin / 2)) .array() - .max(FCL_REAL(0))); + .max(CoalScalar(0))); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = internal::obbDisjoint_check_A_axis(T, a, b, Bf); @@ -396,35 +395,35 @@ bool OBB::overlap(const OBB& other) const { /// compute what transform [R,T] that takes us from cs1 to cs2. /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] /// First compute the rotation part, then translation part - Vec3f T(axes.transpose() * (other.To - To)); - Matrix3f R(axes.transpose() * other.axes); + Vec3s T(axes.transpose() * (other.To - To)); + Matrix3s R(axes.transpose() * other.axes); return !obbDisjoint(R, T, extent, other.extent); } bool OBB::overlap(const OBB& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { /// compute what transform [R,T] that takes us from cs1 to cs2. /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] /// First compute the rotation part, then translation part - // Vec3f t = other.To - To; // T2 - T1 - // Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1) - // Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), + // Vec3s t = other.To - To; // T2 - T1 + // Vec3s T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1) + // Matrix3s R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), // axis[0].dot(other.axis[2]), // axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), // axis[1].dot(other.axis[2]), // axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), // axis[2].dot(other.axis[2])); - Vec3f T(axes.transpose() * (other.To - To)); - Matrix3f R(axes.transpose() * other.axes); + Vec3s T(axes.transpose() * (other.To - To)); + Matrix3s R(axes.transpose() * other.axes); return !obbDisjointAndLowerBoundDistance(R, T, extent, other.extent, request, sqrDistLowerBound); } -bool OBB::contain(const Vec3f& p) const { - Vec3f local_p(p - To); - FCL_REAL proj = local_p.dot(axes.col(0)); +bool OBB::contain(const Vec3s& p) const { + Vec3s local_p(p - To); + CoalScalar proj = local_p.dot(axes.col(0)); if ((proj > extent[0]) || (proj < -extent[0])) return false; proj = local_p.dot(axes.col(1)); @@ -436,7 +435,7 @@ bool OBB::contain(const Vec3f& p) const { return true; } -OBB& OBB::operator+=(const Vec3f& p) { +OBB& OBB::operator+=(const Vec3s& p) { OBB bvp; bvp.To = p; bvp.axes.noalias() = axes; @@ -447,9 +446,9 @@ OBB& OBB::operator+=(const Vec3f& p) { } OBB OBB::operator+(const OBB& other) const { - Vec3f center_diff = To - other.To; - FCL_REAL max_extent = std::max(std::max(extent[0], extent[1]), extent[2]); - FCL_REAL max_extent2 = + Vec3s center_diff = To - other.To; + CoalScalar max_extent = std::max(std::max(extent[0], extent[1]), extent[2]); + CoalScalar max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]); if (center_diff.norm() > 2 * (max_extent + max_extent2)) { return merge_largedist(*this, other); @@ -458,36 +457,35 @@ OBB OBB::operator+(const OBB& other) const { } } -FCL_REAL OBB::distance(const OBB& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/) const { +CoalScalar OBB::distance(const OBB& /*other*/, Vec3s* /*P*/, + Vec3s* /*Q*/) const { std::cerr << "OBB distance not implemented!" << std::endl; return 0.0; } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, +bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2) { - Vec3f Ttemp(R0.transpose() * (b2.To - T0) - b1.To); - Vec3f T(b1.axes.transpose() * Ttemp); - Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes); + Vec3s Ttemp(R0.transpose() * (b2.To - T0) - b1.To); + Vec3s T(b1.axes.transpose() * Ttemp); + Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes); return !obbDisjoint(R, T, b1.extent, b2.extent); } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2, - const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) { - Vec3f Ttemp(R0.transpose() * (b2.To - T0) - b1.To); - Vec3f T(b1.axes.transpose() * Ttemp); - Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes); +bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2, + const CollisionRequest& request, CoalScalar& sqrDistLowerBound) { + Vec3s Ttemp(R0.transpose() * (b2.To - T0) - b1.To); + Vec3s T(b1.axes.transpose() * Ttemp); + Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes); return !obbDisjointAndLowerBoundDistance(R, T, b1.extent, b2.extent, request, sqrDistLowerBound); } -OBB translate(const OBB& bv, const Vec3f& t) { +OBB translate(const OBB& bv, const Vec3s& t) { OBB res(bv); res.To += t; return res; } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BV/OBB.h b/src/BV/OBB.h index 24d6ef877..a7d3a3817 100644 --- a/src/BV/OBB.h +++ b/src/BV/OBB.h @@ -33,21 +33,18 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef HPP_FCL_SRC_OBB_H -#define HPP_FCL_SRC_OBB_H +#ifndef COAL_SRC_OBB_H +#define COAL_SRC_OBB_H -namespace hpp { -namespace fcl { +namespace coal { -bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, +bool obbDisjointAndLowerBoundDistance(const Matrix3s& B, const Vec3s& T, + const Vec3s& a, const Vec3s& b, const CollisionRequest& request, - FCL_REAL& squaredLowerBoundDistance); + CoalScalar& squaredLowerBoundDistance); -bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b); -} // namespace fcl +bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b); +} // namespace coal -} // namespace hpp - -#endif // HPP_FCL_SRC_OBB_H +#endif // COAL_SRC_OBB_H diff --git a/src/BV/OBBRSS.cpp b/src/BV/OBBRSS.cpp index 87a8494e8..1842e60f0 100644 --- a/src/BV/OBBRSS.cpp +++ b/src/BV/OBBRSS.cpp @@ -35,18 +35,15 @@ /** \author Jia Pan */ -#include +#include "coal/BV/OBBRSS.h" -namespace hpp { -namespace fcl { +namespace coal { -OBBRSS translate(const OBBRSS& bv, const Vec3f& t) { +OBBRSS translate(const OBBRSS& bv, const Vec3s& t) { OBBRSS res(bv); res.obb.To += t; res.rss.Tr += t; return res; } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp index 5082775af..97d5d1adb 100644 --- a/src/BV/RSS.cpp +++ b/src/BV/RSS.cpp @@ -35,18 +35,17 @@ /** \author Jia Pan */ -#include -#include -#include -#include +#include "coal/BV/RSS.h" +#include "coal/BVH/BVH_utility.h" +#include "coal/internal/tools.h" +#include "coal/collision_data.h" #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief Clip value between a and b -void clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b) { +void clipToRange(CoalScalar& val, CoalScalar a, CoalScalar b) { if (val < a) val = a; else if (val > b) @@ -64,9 +63,9 @@ void clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b) { /// of each segment. "T" in the dot products is the vector betweeen Pa and Pb. /// Reference: "On fast computation of distance between line segments." Vladimir /// J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985. -void segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b, - FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T) { - FCL_REAL denom = 1 - A_dot_B * A_dot_B; +void segCoords(CoalScalar& t, CoalScalar& u, CoalScalar a, CoalScalar b, + CoalScalar A_dot_B, CoalScalar A_dot_T, CoalScalar B_dot_T) { + CoalScalar denom = 1 - A_dot_B * A_dot_B; if (denom == 0) t = 0; @@ -92,12 +91,12 @@ void segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b, /// Pa + A*t, 0 <= t <= a, is within the half space /// determined by the point Pa and the direction Anorm. /// A,B, and Anorm are unit vectors. T is the vector between Pa and Pb. -bool inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B, - FCL_REAL Anorm_dot_T, FCL_REAL A_dot_B, FCL_REAL A_dot_T, - FCL_REAL B_dot_T) { +bool inVoronoi(CoalScalar a, CoalScalar b, CoalScalar Anorm_dot_B, + CoalScalar Anorm_dot_T, CoalScalar A_dot_B, CoalScalar A_dot_T, + CoalScalar B_dot_T) { if (fabs(Anorm_dot_B) < 1e-7) return false; - FCL_REAL t, u, v; + CoalScalar t, u, v; u = -Anorm_dot_T / Anorm_dot_B; clipToRange(u, 0, b); @@ -118,18 +117,18 @@ bool inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B, /// @brief Distance between two oriented rectangles; P and Q (optional return /// values) are the closest points in the rectangles, both are in the local /// frame of the first rectangle. -FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, - const FCL_REAL a[2], const FCL_REAL b[2], Vec3f* P = NULL, - Vec3f* Q = NULL) { - FCL_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1; +CoalScalar rectDistance(const Matrix3s& Rab, Vec3s const& Tab, + const CoalScalar a[2], const CoalScalar b[2], + Vec3s* P = NULL, Vec3s* Q = NULL) { + CoalScalar A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1; A0_dot_B0 = Rab(0, 0); A0_dot_B1 = Rab(0, 1); A1_dot_B0 = Rab(1, 0); A1_dot_B1 = Rab(1, 1); - FCL_REAL aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1; - FCL_REAL bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1; + CoalScalar aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1; + CoalScalar bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1; aA0_dot_B0 = a[0] * A0_dot_B0; aA0_dot_B1 = a[0] * A0_dot_B1; @@ -140,16 +139,16 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, bA0_dot_B1 = b[1] * A0_dot_B1; bA1_dot_B1 = b[1] * A1_dot_B1; - Vec3f Tba(Rab.transpose() * Tab); + Vec3s Tba(Rab.transpose() * Tab); - Vec3f S; - FCL_REAL t, u; + Vec3s S; + CoalScalar t, u; // determine if any edge pair contains the closest points - FCL_REAL ALL_x, ALU_x, AUL_x, AUU_x; - FCL_REAL BLL_x, BLU_x, BUL_x, BUU_x; - FCL_REAL LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux; + CoalScalar ALL_x, ALU_x, AUL_x, AUU_x; + CoalScalar BLL_x, BLU_x, BUL_x, BUU_x; + CoalScalar LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux; ALL_x = -Tba[0]; ALU_x = ALL_x + aA1_dot_B0; @@ -278,14 +277,14 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, } } - FCL_REAL ALL_y, ALU_y, AUL_y, AUU_y; + CoalScalar ALL_y, ALU_y, AUL_y, AUU_y; ALL_y = -Tba[1]; ALU_y = ALL_y + aA1_dot_B1; AUL_y = ALL_y + aA0_dot_B1; AUU_y = ALU_y + aA0_dot_B1; - FCL_REAL LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux; + CoalScalar LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux; if (ALL_y < ALU_y) { LA1_ly = ALL_y; @@ -405,14 +404,14 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, } } - FCL_REAL BLL_y, BLU_y, BUL_y, BUU_y; + CoalScalar BLL_y, BLU_y, BUL_y, BUU_y; BLL_y = Tab[1]; BLU_y = BLL_y + bA1_dot_B1; BUL_y = BLL_y + bA1_dot_B0; BUU_y = BLU_y + bA1_dot_B0; - FCL_REAL LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy; + CoalScalar LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy; if (ALL_x < AUL_x) { LA0_lx = ALL_x; @@ -531,7 +530,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, } } - FCL_REAL LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy; + CoalScalar LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy; if (ALL_y < AUL_y) { LA0_ly = ALL_y; @@ -653,7 +652,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, // no edges passed, take max separation along face normals - FCL_REAL sep1, sep2; + CoalScalar sep1, sep2; if (Tab[2] > 0.0) { sep1 = Tab[2]; @@ -688,8 +687,8 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, } if (sep2 >= sep1 && sep2 >= 0) { - Vec3f Q_(Tab[0], Tab[1], Tab[2]); - Vec3f P_; + Vec3s Q_(Tab[0], Tab[1], Tab[2]); + Vec3s P_; if (Tba[2] < 0) { P_[0] = Rab(0, 2) * sep2 + Tab[0]; P_[1] = Rab(1, 2) * sep2 + Tab[1]; @@ -708,7 +707,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, } } - FCL_REAL sep = (sep1 > sep2 ? sep1 : sep2); + CoalScalar sep = (sep1 > sep2 ? sep1 : sep2); return (sep > 0 ? sep : 0); } @@ -718,56 +717,56 @@ bool RSS::overlap(const RSS& other) const { /// First compute the rotation part, then translation part /// Then compute R1'(T2 - T1) - Vec3f T(axes.transpose() * (other.Tr - Tr)); + Vec3s T(axes.transpose() * (other.Tr - Tr)); /// Now compute R1'R2 - Matrix3f R(axes.transpose() * other.axes); + Matrix3s R(axes.transpose() * other.axes); - FCL_REAL dist = rectDistance(R, T, length, other.length); + CoalScalar dist = rectDistance(R, T, length, other.length); return (dist <= (radius + other.radius)); } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, +bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, const RSS& b2) { // ROb2 = R0 . b2 // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ] // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0] // R = b2^T RO^T b1 - Vec3f Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr); - Vec3f T(b1.axes.transpose() * Ttemp); - Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes); + Vec3s Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr); + Vec3s T(b1.axes.transpose() * Ttemp); + Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes); - FCL_REAL dist = rectDistance(R, T, b1.length, b2.length); + CoalScalar dist = rectDistance(R, T, b1.length, b2.length); return (dist <= (b1.radius + b2.radius)); } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, - const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) { +bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, const RSS& b2, + const CollisionRequest& request, CoalScalar& sqrDistLowerBound) { // ROb2 = R0 . b2 // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ] // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0] // R = b2^T RO^T b1 - Vec3f Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr); - Vec3f T(b1.axes.transpose() * Ttemp); - Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes); + Vec3s Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr); + Vec3s T(b1.axes.transpose() * Ttemp); + Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes); - FCL_REAL dist = rectDistance(R, T, b1.length, b2.length) - b1.radius - - b2.radius - request.security_margin; + CoalScalar dist = rectDistance(R, T, b1.length, b2.length) - b1.radius - + b2.radius - request.security_margin; if (dist <= 0) return true; sqrDistLowerBound = dist * dist; return false; } -bool RSS::contain(const Vec3f& p) const { - Vec3f local_p = p - Tr; - // FIXME: Vec3f proj (axes.transpose() * local_p); - FCL_REAL proj0 = local_p.dot(axes.col(0)); - FCL_REAL proj1 = local_p.dot(axes.col(1)); - FCL_REAL proj2 = local_p.dot(axes.col(2)); - FCL_REAL abs_proj2 = fabs(proj2); - Vec3f proj(proj0, proj1, proj2); +bool RSS::contain(const Vec3s& p) const { + Vec3s local_p = p - Tr; + // FIXME: Vec3s proj (axes.transpose() * local_p); + CoalScalar proj0 = local_p.dot(axes.col(0)); + CoalScalar proj1 = local_p.dot(axes.col(1)); + CoalScalar proj2 = local_p.dot(axes.col(2)); + CoalScalar abs_proj2 = fabs(proj2); + Vec3s proj(proj0, proj1, proj2); /// projection is within the rectangle if ((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && @@ -775,29 +774,29 @@ bool RSS::contain(const Vec3f& p) const { return (abs_proj2 < radius); } else if ((proj0 < length[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > length[1]))) { - FCL_REAL y = (proj1 > 0) ? length[1] : 0; - Vec3f v(proj0, y, 0); + CoalScalar y = (proj1 > 0) ? length[1] : 0; + Vec3s v(proj0, y, 0); return ((proj - v).squaredNorm() < radius * radius); } else if ((proj1 < length[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > length[0]))) { - FCL_REAL x = (proj0 > 0) ? length[0] : 0; - Vec3f v(x, proj1, 0); + CoalScalar x = (proj0 > 0) ? length[0] : 0; + Vec3s v(x, proj1, 0); return ((proj - v).squaredNorm() < radius * radius); } else { - FCL_REAL x = (proj0 > 0) ? length[0] : 0; - FCL_REAL y = (proj1 > 0) ? length[1] : 0; - Vec3f v(x, y, 0); + CoalScalar x = (proj0 > 0) ? length[0] : 0; + CoalScalar y = (proj1 > 0) ? length[1] : 0; + Vec3s v(x, y, 0); return ((proj - v).squaredNorm() < radius * radius); } } -RSS& RSS::operator+=(const Vec3f& p) { - Vec3f local_p = p - Tr; - FCL_REAL proj0 = local_p.dot(axes.col(0)); - FCL_REAL proj1 = local_p.dot(axes.col(1)); - FCL_REAL proj2 = local_p.dot(axes.col(2)); - FCL_REAL abs_proj2 = fabs(proj2); - Vec3f proj(proj0, proj1, proj2); +RSS& RSS::operator+=(const Vec3s& p) { + Vec3s local_p = p - Tr; + CoalScalar proj0 = local_p.dot(axes.col(0)); + CoalScalar proj1 = local_p.dot(axes.col(1)); + CoalScalar proj2 = local_p.dot(axes.col(2)); + CoalScalar abs_proj2 = fabs(proj2); + Vec3s proj(proj0, proj1, proj2); // projection is within the rectangle if ((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && @@ -814,19 +813,19 @@ RSS& RSS::operator+=(const Vec3f& p) { } } else if ((proj0 < length[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > length[1]))) { - FCL_REAL y = (proj1 > 0) ? length[1] : 0; - Vec3f v(proj0, y, 0); - FCL_REAL new_r_sqr = (proj - v).squaredNorm(); + CoalScalar y = (proj1 > 0) ? length[1] : 0; + Vec3s v(proj0, y, 0); + CoalScalar new_r_sqr = (proj - v).squaredNorm(); if (new_r_sqr < radius * radius) ; // do nothing else { if (abs_proj2 < radius) { - FCL_REAL delta_y = + CoalScalar delta_y = -std::sqrt(radius * radius - proj2 * proj2) + fabs(proj1 - y); length[1] += delta_y; if (proj1 < 0) Tr[1] -= delta_y; } else { - FCL_REAL delta_y = fabs(proj1 - y); + CoalScalar delta_y = fabs(proj1 - y); length[1] += delta_y; if (proj1 < 0) Tr[1] -= delta_y; @@ -838,19 +837,19 @@ RSS& RSS::operator+=(const Vec3f& p) { } } else if ((proj1 < length[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > length[0]))) { - FCL_REAL x = (proj0 > 0) ? length[0] : 0; - Vec3f v(x, proj1, 0); - FCL_REAL new_r_sqr = (proj - v).squaredNorm(); + CoalScalar x = (proj0 > 0) ? length[0] : 0; + Vec3s v(x, proj1, 0); + CoalScalar new_r_sqr = (proj - v).squaredNorm(); if (new_r_sqr < radius * radius) ; // do nothing else { if (abs_proj2 < radius) { - FCL_REAL delta_x = + CoalScalar delta_x = -std::sqrt(radius * radius - proj2 * proj2) + fabs(proj0 - x); length[0] += delta_x; if (proj0 < 0) Tr[0] -= delta_x; } else { - FCL_REAL delta_x = fabs(proj0 - x); + CoalScalar delta_x = fabs(proj0 - x); length[0] += delta_x; if (proj0 < 0) Tr[0] -= delta_x; @@ -861,20 +860,20 @@ RSS& RSS::operator+=(const Vec3f& p) { } } } else { - FCL_REAL x = (proj0 > 0) ? length[0] : 0; - FCL_REAL y = (proj1 > 0) ? length[1] : 0; - Vec3f v(x, y, 0); - FCL_REAL new_r_sqr = (proj - v).squaredNorm(); + CoalScalar x = (proj0 > 0) ? length[0] : 0; + CoalScalar y = (proj1 > 0) ? length[1] : 0; + Vec3s v(x, y, 0); + CoalScalar new_r_sqr = (proj - v).squaredNorm(); if (new_r_sqr < radius * radius) ; // do nothing else { if (abs_proj2 < radius) { - FCL_REAL diag = std::sqrt(new_r_sqr - proj2 * proj2); - FCL_REAL delta_diag = + CoalScalar diag = std::sqrt(new_r_sqr - proj2 * proj2); + CoalScalar delta_diag = -std::sqrt(radius * radius - proj2 * proj2) + diag; - FCL_REAL delta_x = delta_diag / diag * fabs(proj0 - x); - FCL_REAL delta_y = delta_diag / diag * fabs(proj1 - y); + CoalScalar delta_x = delta_diag / diag * fabs(proj0 - x); + CoalScalar delta_y = delta_diag / diag * fabs(proj1 - y); length[0] += delta_x; length[1] += delta_y; @@ -883,8 +882,8 @@ RSS& RSS::operator+=(const Vec3f& p) { Tr[1] -= delta_y; } } else { - FCL_REAL delta_x = fabs(proj0 - x); - FCL_REAL delta_y = fabs(proj1 - y); + CoalScalar delta_x = fabs(proj0 - x); + CoalScalar delta_y = fabs(proj1 - y); length[0] += delta_x; length[1] += delta_y; @@ -908,13 +907,13 @@ RSS& RSS::operator+=(const Vec3f& p) { RSS RSS::operator+(const RSS& other) const { RSS bv; - Vec3f v[16]; - Vec3f d0_pos(other.axes.col(0) * (other.length[0] + other.radius)); - Vec3f d1_pos(other.axes.col(1) * (other.length[1] + other.radius)); - Vec3f d0_neg(other.axes.col(0) * (-other.radius)); - Vec3f d1_neg(other.axes.col(1) * (-other.radius)); - Vec3f d2_pos(other.axes.col(2) * other.radius); - Vec3f d2_neg(other.axes.col(2) * (-other.radius)); + Vec3s v[16]; + Vec3s d0_pos(other.axes.col(0) * (other.length[0] + other.radius)); + Vec3s d1_pos(other.axes.col(1) * (other.length[1] + other.radius)); + Vec3s d0_neg(other.axes.col(0) * (-other.radius)); + Vec3s d1_neg(other.axes.col(1) * (-other.radius)); + Vec3s d2_pos(other.axes.col(2) * other.radius); + Vec3s d2_neg(other.axes.col(2) * (-other.radius)); v[0].noalias() = other.Tr + d0_pos + d1_pos + d2_pos; v[1].noalias() = other.Tr + d0_pos + d1_pos + d2_neg; @@ -941,9 +940,9 @@ RSS RSS::operator+(const RSS& other) const { v[14].noalias() = Tr + d0_neg + d1_neg + d2_pos; v[15].noalias() = Tr + d0_neg + d1_neg + d2_neg; - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors - Matrix3f::Scalar s[3] = {0, 0, 0}; + Matrix3s M; // row first matrix + Vec3s E[3]; // row first eigen-vectors + CoalScalar s[3] = {0, 0, 0}; getCovariance(v, NULL, NULL, NULL, 16, M); eigen(M, s, E); @@ -980,36 +979,34 @@ RSS RSS::operator+(const RSS& other) const { return bv; } -FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const { +CoalScalar RSS::distance(const RSS& other, Vec3s* P, Vec3s* Q) const { // compute what transform [R,T] that takes us from cs1 to cs2. // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] // First compute the rotation part, then translation part - Matrix3f R(axes.transpose() * other.axes); - Vec3f T(axes.transpose() * (other.Tr - Tr)); + Matrix3s R(axes.transpose() * other.axes); + Vec3s T(axes.transpose() * (other.Tr - Tr)); - FCL_REAL dist = rectDistance(R, T, length, other.length, P, Q); + CoalScalar dist = rectDistance(R, T, length, other.length, P, Q); dist -= (radius + other.radius); - return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist; + return (dist < (CoalScalar)0.0) ? (CoalScalar)0.0 : dist; } -FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, - const RSS& b2, Vec3f* P, Vec3f* Q) { - Matrix3f R(b1.axes.transpose() * R0 * b2.axes); - Vec3f Ttemp(R0 * b2.Tr + T0 - b1.Tr); +CoalScalar distance(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, + const RSS& b2, Vec3s* P, Vec3s* Q) { + Matrix3s R(b1.axes.transpose() * R0 * b2.axes); + Vec3s Ttemp(R0 * b2.Tr + T0 - b1.Tr); - Vec3f T(b1.axes.transpose() * Ttemp); + Vec3s T(b1.axes.transpose() * Ttemp); - FCL_REAL dist = rectDistance(R, T, b1.length, b2.length, P, Q); + CoalScalar dist = rectDistance(R, T, b1.length, b2.length, P, Q); dist -= (b1.radius + b2.radius); - return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist; + return (dist < (CoalScalar)0.0) ? (CoalScalar)0.0 : dist; } -RSS translate(const RSS& bv, const Vec3f& t) { +RSS translate(const RSS& bv, const Vec3s& t) { RSS res(bv); res.Tr += t; return res; } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BV/kDOP.cpp b/src/BV/kDOP.cpp index 3e92cb30d..d59ccc6b9 100644 --- a/src/BV/kDOP.cpp +++ b/src/BV/kDOP.cpp @@ -35,17 +35,17 @@ /** \author Jia Pan */ -#include +#include "coal/collision_data.h" +#include "coal/BV/kDOP.h" + #include #include -#include - -namespace hpp { -namespace fcl { +namespace coal { /// @brief Find the smaller and larger one of two values -inline void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL& minv, FCL_REAL& maxv) { +inline void minmax(CoalScalar a, CoalScalar b, CoalScalar& minv, + CoalScalar& maxv) { if (a > b) { minv = b; maxv = a; @@ -55,7 +55,7 @@ inline void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL& minv, FCL_REAL& maxv) { } } /// @brief Merge the interval [minv, maxv] and value p/ -inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv) { +inline void minmax(CoalScalar p, CoalScalar& minv, CoalScalar& maxv) { if (p > maxv) maxv = p; if (p < minv) minv = p; } @@ -63,11 +63,11 @@ inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv) { /// @brief Compute the distances to planes with normals from KDOP vectors except /// those of AABB face planes template -void getDistances(const Vec3f& /*p*/, FCL_REAL* /*d*/) {} +void getDistances(const Vec3s& /*p*/, CoalScalar* /*d*/) {} /// @brief Specification of getDistances template <> -inline void getDistances<5>(const Vec3f& p, FCL_REAL* d) { +inline void getDistances<5>(const Vec3s& p, CoalScalar* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; @@ -76,7 +76,7 @@ inline void getDistances<5>(const Vec3f& p, FCL_REAL* d) { } template <> -inline void getDistances<6>(const Vec3f& p, FCL_REAL* d) { +inline void getDistances<6>(const Vec3s& p, CoalScalar* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; @@ -86,7 +86,7 @@ inline void getDistances<6>(const Vec3f& p, FCL_REAL* d) { } template <> -inline void getDistances<9>(const Vec3f& p, FCL_REAL* d) { +inline void getDistances<9>(const Vec3s& p, CoalScalar* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; @@ -100,18 +100,18 @@ inline void getDistances<9>(const Vec3f& p, FCL_REAL* d) { template KDOP::KDOP() { - FCL_REAL real_max = (std::numeric_limits::max)(); + CoalScalar real_max = (std::numeric_limits::max)(); dist_.template head().setConstant(real_max); dist_.template tail().setConstant(-real_max); } template -KDOP::KDOP(const Vec3f& v) { +KDOP::KDOP(const Vec3s& v) { for (short i = 0; i < 3; ++i) { dist_[i] = dist_[N / 2 + i] = v[i]; } - FCL_REAL d[(N - 6) / 2]; + CoalScalar d[(N - 6) / 2]; getDistances<(N - 6) / 2>(v, d); for (short i = 0; i < (N - 6) / 2; ++i) { dist_[3 + i] = dist_[3 + i + N / 2] = d[i]; @@ -119,12 +119,12 @@ KDOP::KDOP(const Vec3f& v) { } template -KDOP::KDOP(const Vec3f& a, const Vec3f& b) { +KDOP::KDOP(const Vec3s& a, const Vec3s& b) { for (short i = 0; i < 3; ++i) { minmax(a[i], b[i], dist_[i], dist_[i + N / 2]); } - FCL_REAL ad[(N - 6) / 2], bd[(N - 6) / 2]; + CoalScalar ad[(N - 6) / 2], bd[(N - 6) / 2]; getDistances<(N - 6) / 2>(a, ad); getDistances<(N - 6) / 2>(b, bd); for (short i = 0; i < (N - 6) / 2; ++i) { @@ -143,11 +143,11 @@ bool KDOP::overlap(const KDOP& other) const { template bool KDOP::overlap(const KDOP& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const { - const FCL_REAL breakDistance(request.break_distance + - request.security_margin); + CoalScalar& sqrDistLowerBound) const { + const CoalScalar breakDistance(request.break_distance + + request.security_margin); - FCL_REAL a = + CoalScalar a = (dist_.template head() - other.dist_.template tail()) .minCoeff(); if (a > breakDistance) { @@ -155,7 +155,7 @@ bool KDOP::overlap(const KDOP& other, const CollisionRequest& request, return false; } - FCL_REAL b = + CoalScalar b = (other.dist_.template head() - dist_.template tail()) .minCoeff(); if (b > breakDistance) { @@ -168,12 +168,12 @@ bool KDOP::overlap(const KDOP& other, const CollisionRequest& request, } template -bool KDOP::inside(const Vec3f& p) const { +bool KDOP::inside(const Vec3s& p) const { if ((p.array() < dist_.template head<3>()).any()) return false; if ((p.array() > dist_.template segment<3>(N / 2)).any()) return false; enum { P = ((N - 6) / 2) }; - Eigen::Array d; + Eigen::Array d; getDistances

(p, d.data()); if ((d < dist_.template segment

(3)).any()) return false; @@ -183,12 +183,12 @@ bool KDOP::inside(const Vec3f& p) const { } template -KDOP& KDOP::operator+=(const Vec3f& p) { +KDOP& KDOP::operator+=(const Vec3s& p) { for (short i = 0; i < 3; ++i) { minmax(p[i], dist_[i], dist_[N / 2 + i]); } - FCL_REAL pd[(N - 6) / 2]; + CoalScalar pd[(N - 6) / 2]; getDistances<(N - 6) / 2>(p, pd); for (short i = 0; i < (N - 6) / 2; ++i) { minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]); @@ -213,21 +213,21 @@ KDOP KDOP::operator+(const KDOP& other) const { } template -FCL_REAL KDOP::distance(const KDOP& /*other*/, Vec3f* /*P*/, - Vec3f* /*Q*/) const { +CoalScalar KDOP::distance(const KDOP& /*other*/, Vec3s* /*P*/, + Vec3s* /*Q*/) const { std::cerr << "KDOP distance not implemented!" << std::endl; return 0.0; } template -KDOP translate(const KDOP& bv, const Vec3f& t) { +KDOP translate(const KDOP& bv, const Vec3s& t) { KDOP res(bv); for (short i = 0; i < 3; ++i) { res.dist(i) += t[i]; res.dist(short(N / 2 + i)) += t[i]; } - FCL_REAL d[(N - 6) / 2]; + CoalScalar d[(N - 6) / 2]; getDistances<(N - 6) / 2>(t, d); for (short i = 0; i < (N - 6) / 2; ++i) { res.dist(short(3 + i)) += d[i]; @@ -241,10 +241,8 @@ template class KDOP<16>; template class KDOP<18>; template class KDOP<24>; -template KDOP<16> translate<16>(const KDOP<16>&, const Vec3f&); -template KDOP<18> translate<18>(const KDOP<18>&, const Vec3f&); -template KDOP<24> translate<24>(const KDOP<24>&, const Vec3f&); - -} // namespace fcl +template KDOP<16> translate<16>(const KDOP<16>&, const Vec3s&); +template KDOP<18> translate<18>(const KDOP<18>&, const Vec3s&); +template KDOP<24> translate<24>(const KDOP<24>&, const Vec3s&); -} // namespace hpp +} // namespace coal diff --git a/src/BV/kIOS.cpp b/src/BV/kIOS.cpp index 72ba3523d..802496e08 100644 --- a/src/BV/kIOS.cpp +++ b/src/BV/kIOS.cpp @@ -35,21 +35,20 @@ /** \author Jia Pan */ -#include -#include -#include +#include "coal/BV/kIOS.h" +#include "coal/BVH/BVH_utility.h" +#include "coal/math/transform.h" #include #include -namespace hpp { -namespace fcl { +namespace coal { bool kIOS::overlap(const kIOS& other) const { for (unsigned int i = 0; i < num_spheres; ++i) { for (unsigned int j = 0; j < other.num_spheres; ++j) { - FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); - FCL_REAL sum_r = spheres[i].r + other.spheres[j].r; + CoalScalar o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); + CoalScalar sum_r = spheres[i].r + other.spheres[j].r; if (o_dist > sum_r * sum_r) return false; } } @@ -58,11 +57,11 @@ bool kIOS::overlap(const kIOS& other) const { } bool kIOS::overlap(const kIOS& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { for (unsigned int i = 0; i < num_spheres; ++i) { for (unsigned int j = 0; j < other.num_spheres; ++j) { - FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); - FCL_REAL sum_r = spheres[i].r + other.spheres[j].r; + CoalScalar o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); + CoalScalar sum_r = spheres[i].r + other.spheres[j].r; if (o_dist > sum_r * sum_r) { o_dist = sqrt(o_dist) - sum_r; sqrDistLowerBound = o_dist * o_dist; @@ -74,19 +73,19 @@ bool kIOS::overlap(const kIOS& other, const CollisionRequest& request, return obb.overlap(other.obb, request, sqrDistLowerBound); } -bool kIOS::contain(const Vec3f& p) const { +bool kIOS::contain(const Vec3s& p) const { for (unsigned int i = 0; i < num_spheres; ++i) { - FCL_REAL r = spheres[i].r; + CoalScalar r = spheres[i].r; if ((spheres[i].o - p).squaredNorm() > r * r) return false; } return true; } -kIOS& kIOS::operator+=(const Vec3f& p) { +kIOS& kIOS::operator+=(const Vec3s& p) { for (unsigned int i = 0; i < num_spheres; ++i) { - FCL_REAL r = spheres[i].r; - FCL_REAL new_r_sqr = (p - spheres[i].o).squaredNorm(); + CoalScalar r = spheres[i].r; + CoalScalar new_r_sqr = (p - spheres[i].o).squaredNorm(); if (new_r_sqr > r * r) { spheres[i].r = sqrt(new_r_sqr); } @@ -110,23 +109,23 @@ kIOS kIOS::operator+(const kIOS& other) const { return result; } -FCL_REAL kIOS::width() const { return obb.width(); } +CoalScalar kIOS::width() const { return obb.width(); } -FCL_REAL kIOS::height() const { return obb.height(); } +CoalScalar kIOS::height() const { return obb.height(); } -FCL_REAL kIOS::depth() const { return obb.depth(); } +CoalScalar kIOS::depth() const { return obb.depth(); } -FCL_REAL kIOS::volume() const { return obb.volume(); } +CoalScalar kIOS::volume() const { return obb.volume(); } -FCL_REAL kIOS::size() const { return volume(); } +CoalScalar kIOS::size() const { return volume(); } -FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const { - FCL_REAL d_max = 0; +CoalScalar kIOS::distance(const kIOS& other, Vec3s* P, Vec3s* Q) const { + CoalScalar d_max = 0; long id_a = -1, id_b = -1; for (unsigned int i = 0; i < num_spheres; ++i) { for (unsigned int j = 0; j < other.num_spheres; ++j) { - FCL_REAL d = (spheres[i].o - other.spheres[j].o).norm() - - (spheres[i].r + other.spheres[j].r); + CoalScalar d = (spheres[i].o - other.spheres[j].o).norm() - + (spheres[i].r + other.spheres[j].r); if (d_max < d) { d_max = d; if (P && Q) { @@ -139,8 +138,8 @@ FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const { if (P && Q) { if (id_a != -1 && id_b != -1) { - const Vec3f v = spheres[id_a].o - spheres[id_b].o; - FCL_REAL len_v = v.norm(); + const Vec3s v = spheres[id_a].o - spheres[id_b].o; + CoalScalar len_v = v.norm(); *P = spheres[id_a].o - v * (spheres[id_a].r / len_v); *Q = spheres[id_b].o + v * (spheres[id_b].r / len_v); } @@ -149,7 +148,7 @@ FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const { return d_max; } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, +bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, const kIOS& b2) { kIOS b2_temp = b2; for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { @@ -163,9 +162,9 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, return b1.overlap(b2_temp); } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, +bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, const kIOS& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) { + CoalScalar& sqrDistLowerBound) { kIOS b2_temp = b2; for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { b2_temp.spheres[i].o.noalias() = @@ -178,8 +177,8 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, return b1.overlap(b2_temp, request, sqrDistLowerBound); } -FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, - const kIOS& b2, Vec3f* P, Vec3f* Q) { +CoalScalar distance(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, + const kIOS& b2, Vec3s* P, Vec3s* Q) { kIOS b2_temp = b2; for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0; @@ -188,7 +187,7 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, return b1.distance(b2_temp, P, Q); } -kIOS translate(const kIOS& bv, const Vec3f& t) { +kIOS translate(const kIOS& bv, const Vec3s& t) { kIOS res(bv); for (size_t i = 0; i < res.num_spheres; ++i) { res.spheres[i].o += t; @@ -198,6 +197,4 @@ kIOS translate(const kIOS& bv, const Vec3f& t) { return res; } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp index 3327fd071..9cfdcab99 100644 --- a/src/BVH/BVH_model.cpp +++ b/src/BVH/BVH_model.cpp @@ -36,20 +36,19 @@ /** \author Jia Pan */ -#include "hpp/fcl/BV/BV_node.h" -#include +#include "coal/BV/BV_node.h" +#include "coal/BVH/BVH_model.h" -#include -#include +#include "coal/BV/BV.h" +#include "coal/shape/convex.h" -#include -#include +#include "coal/internal/BV_splitter.h" +#include "coal/internal/BV_fitter.h" -#include -#include +#include +#include -namespace hpp { -namespace fcl { +namespace coal { BVHModelBase::BVHModelBase() : num_tris(0), @@ -67,7 +66,7 @@ BVHModelBase::BVHModelBase(const BVHModelBase& other) num_tris_allocated(other.num_tris), num_vertices_allocated(other.num_vertices) { if (other.vertices.get() && other.vertices->size() > 0) { - vertices.reset(new std::vector(*(other.vertices))); + vertices.reset(new std::vector(*(other.vertices))); } else vertices.reset(); @@ -77,7 +76,7 @@ BVHModelBase::BVHModelBase(const BVHModelBase& other) tri_indices.reset(); if (other.prev_vertices.get() && other.prev_vertices->size() > 0) { - prev_vertices.reset(new std::vector(*(other.prev_vertices))); + prev_vertices.reset(new std::vector(*(other.prev_vertices))); } else prev_vertices.reset(); } @@ -106,8 +105,8 @@ bool BVHModelBase::isEqual(const CollisionGeometry& _other) const { (vertices.get() && !(other.vertices.get()))) return false; if (vertices.get() && other.vertices.get()) { - const std::vector& vertices_ = *(vertices); - const std::vector& other_vertices_ = *(other.vertices); + const std::vector& vertices_ = *(vertices); + const std::vector& other_vertices_ = *(other.vertices); for (size_t k = 0; k < static_cast(num_vertices); ++k) if (vertices_[k] != other_vertices_[k]) return false; } @@ -116,8 +115,8 @@ bool BVHModelBase::isEqual(const CollisionGeometry& _other) const { (prev_vertices.get() && !(other.prev_vertices.get()))) return false; if (prev_vertices.get() && other.prev_vertices.get()) { - const std::vector& prev_vertices_ = *(prev_vertices); - const std::vector& other_prev_vertices_ = *(other.prev_vertices); + const std::vector& prev_vertices_ = *(prev_vertices); + const std::vector& other_prev_vertices_ = *(other.prev_vertices); for (size_t k = 0; k < static_cast(num_vertices); ++k) { if (prev_vertices_[k] != other_prev_vertices_[k]) return false; } @@ -141,10 +140,10 @@ void BVHModelBase::buildConvexRepresentation(bool share_memory) { } if (!convex) { - std::shared_ptr> points = vertices; + std::shared_ptr> points = vertices; std::shared_ptr> polygons = tri_indices; if (!share_memory) { - points.reset(new std::vector(*(vertices))); + points.reset(new std::vector(*(vertices))); polygons.reset(new std::vector(*(tri_indices))); } convex.reset( @@ -207,7 +206,7 @@ int BVHModelBase::beginModel(unsigned int num_tris_, tri_indices.reset(); if (num_vertices_allocated > 0) { - vertices.reset(new std::vector(num_vertices_allocated)); + vertices.reset(new std::vector(num_vertices_allocated)); if (!(vertices.get())) { std::cerr << "BVH Error! Out of memory for vertices array on BeginModel() call!" @@ -233,7 +232,7 @@ int BVHModelBase::beginModel(unsigned int num_tris_, return BVH_OK; } -int BVHModelBase::addVertex(const Vec3f& p) { +int BVHModelBase::addVertex(const Vec3s& p) { if (build_state != BVH_BUILD_STATE_BEGUN) { std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertex() " "was ignored. Must do a beginModel() to clear the model for " @@ -243,8 +242,8 @@ int BVHModelBase::addVertex(const Vec3f& p) { } if (num_vertices >= num_vertices_allocated) { - std::shared_ptr> temp( - new std::vector(num_vertices_allocated * 2)); + std::shared_ptr> temp( + new std::vector(num_vertices_allocated * 2)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for vertices array on addVertex() call!" @@ -305,7 +304,7 @@ int BVHModelBase::addTriangles(const Matrixx3i& triangles) { return BVH_OK; } -int BVHModelBase::addVertices(const Matrixx3f& points) { +int BVHModelBase::addVertices(const MatrixX3s& points) { if (build_state != BVH_BUILD_STATE_BEGUN) { std::cerr << "BVH Warning! Call addVertex() in a wrong order. " "addVertices() was ignored. Must do a beginModel() to clear " @@ -316,8 +315,8 @@ int BVHModelBase::addVertices(const Matrixx3f& points) { if (num_vertices + points.rows() > num_vertices_allocated) { num_vertices_allocated = num_vertices + (unsigned int)points.rows(); - std::shared_ptr> temp( - new std::vector(num_vertices_allocated)); + std::shared_ptr> temp( + new std::vector(num_vertices_allocated)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for vertices array on addVertex() call!" @@ -331,15 +330,15 @@ int BVHModelBase::addVertices(const Matrixx3f& points) { vertices = temp; } - std::vector& vertices_ = *vertices; + std::vector& vertices_ = *vertices; for (Eigen::DenseIndex id = 0; id < points.rows(); ++id) vertices_[num_vertices++] = points.row(id).transpose(); return BVH_OK; } -int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2, - const Vec3f& p3) { +int BVHModelBase::addTriangle(const Vec3s& p1, const Vec3s& p2, + const Vec3s& p3) { if (build_state == BVH_BUILD_STATE_PROCESSED) { std::cerr << "BVH Warning! Call addTriangle() in a wrong order. " "addTriangle() was ignored. Must do a beginModel() to clear " @@ -349,8 +348,8 @@ int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2, } if (num_vertices + 2 >= num_vertices_allocated) { - std::shared_ptr> temp( - new std::vector(num_vertices_allocated * 2 + 2)); + std::shared_ptr> temp( + new std::vector(num_vertices_allocated * 2 + 2)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for vertices array on " "addTriangle() call!" @@ -399,7 +398,7 @@ int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2, return BVH_OK; } -int BVHModelBase::addSubModel(const std::vector& ps) { +int BVHModelBase::addSubModel(const std::vector& ps) { if (build_state == BVH_BUILD_STATE_PROCESSED) { std::cerr << "BVH Warning! Calling addSubModel() in a wrong order. " "addSubModel() was ignored. Must do a beginModel() to clear " @@ -411,7 +410,7 @@ int BVHModelBase::addSubModel(const std::vector& ps) { const unsigned int num_vertices_to_add = (unsigned int)ps.size(); if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) { - std::shared_ptr> temp(new std::vector( + std::shared_ptr> temp(new std::vector( num_vertices_allocated * 2 + num_vertices_to_add - 1)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for vertices array on " @@ -428,7 +427,7 @@ int BVHModelBase::addSubModel(const std::vector& ps) { num_vertices_allocated * 2 + num_vertices_to_add - 1; } - std::vector& vertices_ = *vertices; + std::vector& vertices_ = *vertices; for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) { vertices_[num_vertices] = ps[i]; num_vertices++; @@ -437,7 +436,7 @@ int BVHModelBase::addSubModel(const std::vector& ps) { return BVH_OK; } -int BVHModelBase::addSubModel(const std::vector& ps, +int BVHModelBase::addSubModel(const std::vector& ps, const std::vector& ts) { if (build_state == BVH_BUILD_STATE_PROCESSED) { std::cerr << "BVH Warning! Calling addSubModel() in a wrong order. " @@ -450,7 +449,7 @@ int BVHModelBase::addSubModel(const std::vector& ps, const unsigned int num_vertices_to_add = (unsigned int)ps.size(); if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) { - std::shared_ptr> temp(new std::vector( + std::shared_ptr> temp(new std::vector( num_vertices_allocated * 2 + num_vertices_to_add - 1)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for vertices array on " @@ -469,7 +468,7 @@ int BVHModelBase::addSubModel(const std::vector& ps, const unsigned int offset = num_vertices; - std::vector& vertices_ = *vertices; + std::vector& vertices_ = *vertices; for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) { vertices_[num_vertices] = ps[i]; num_vertices++; @@ -544,8 +543,8 @@ int BVHModelBase::endModel() { if (num_vertices_allocated > num_vertices) { if (num_vertices > 0) { - std::shared_ptr> new_vertices( - new std::vector(num_vertices)); + std::shared_ptr> new_vertices( + new std::vector(num_vertices)); if (!(new_vertices.get())) { std::cerr << "BVH Error! Out of memory for vertices array in endModel() call!" @@ -592,7 +591,7 @@ int BVHModelBase::beginReplaceModel() { return BVH_OK; } -int BVHModelBase::replaceVertex(const Vec3f& p) { +int BVHModelBase::replaceVertex(const Vec3s& p) { if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. " "replaceVertex() was ignored. Must do a beginReplaceModel() " @@ -607,8 +606,8 @@ int BVHModelBase::replaceVertex(const Vec3f& p) { return BVH_OK; } -int BVHModelBase::replaceTriangle(const Vec3f& p1, const Vec3f& p2, - const Vec3f& p3) { +int BVHModelBase::replaceTriangle(const Vec3s& p1, const Vec3s& p2, + const Vec3s& p3) { if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. " "replaceTriangle() was ignored. Must do a beginReplaceModel() " @@ -626,7 +625,7 @@ int BVHModelBase::replaceTriangle(const Vec3f& p1, const Vec3f& p2, return BVH_OK; } -int BVHModelBase::replaceSubModel(const std::vector& ps) { +int BVHModelBase::replaceSubModel(const std::vector& ps) { if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. " "replaceSubModel() was ignored. Must do a beginReplaceModel() " @@ -635,7 +634,7 @@ int BVHModelBase::replaceSubModel(const std::vector& ps) { return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } - std::vector& vertices_ = *vertices; + std::vector& vertices_ = *vertices; for (unsigned int i = 0; i < ps.size(); ++i) { vertices_[num_vertex_updated] = ps[i]; num_vertex_updated++; @@ -681,12 +680,12 @@ int BVHModelBase::beginUpdateModel() { } if (prev_vertices.get()) { - std::shared_ptr> temp = prev_vertices; + std::shared_ptr> temp = prev_vertices; prev_vertices = vertices; vertices = temp; } else { prev_vertices = vertices; - vertices.reset(new std::vector(num_vertices)); + vertices.reset(new std::vector(num_vertices)); } num_vertex_updated = 0; @@ -696,7 +695,7 @@ int BVHModelBase::beginUpdateModel() { return BVH_OK; } -int BVHModelBase::updateVertex(const Vec3f& p) { +int BVHModelBase::updateVertex(const Vec3s& p) { if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { std::cerr << "BVH Warning! Call updateVertex() in a wrong order. updateVertex() " @@ -711,8 +710,8 @@ int BVHModelBase::updateVertex(const Vec3f& p) { return BVH_OK; } -int BVHModelBase::updateTriangle(const Vec3f& p1, const Vec3f& p2, - const Vec3f& p3) { +int BVHModelBase::updateTriangle(const Vec3s& p1, const Vec3s& p2, + const Vec3s& p3) { if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. " "updateTriangle() was ignored. Must do a beginUpdateModel() " @@ -730,7 +729,7 @@ int BVHModelBase::updateTriangle(const Vec3f& p1, const Vec3f& p2, return BVH_OK; } -int BVHModelBase::updateSubModel(const std::vector& ps) { +int BVHModelBase::updateSubModel(const std::vector& ps) { if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. " "updateSubModel() was ignored. Must do a beginUpdateModel() " @@ -739,7 +738,7 @@ int BVHModelBase::updateSubModel(const std::vector& ps) { return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } - std::vector& vertices_ = *vertices; + std::vector& vertices_ = *vertices; for (unsigned int i = 0; i < ps.size(); ++i) { vertices_[num_vertex_updated] = ps[i]; num_vertex_updated++; @@ -781,7 +780,7 @@ int BVHModelBase::endUpdateModel(bool refit, bool bottomup) { void BVHModelBase::computeLocalAABB() { AABB aabb_; - const std::vector& vertices_ = *vertices; + const std::vector& vertices_ = *vertices; for (unsigned int i = 0; i < num_vertices; ++i) { aabb_ += vertices_[i]; } @@ -790,7 +789,7 @@ void BVHModelBase::computeLocalAABB() { aabb_radius = 0; for (unsigned int i = 0; i < num_vertices; ++i) { - FCL_REAL r = (aabb_center - vertices_[i]).squaredNorm(); + CoalScalar r = (aabb_center - vertices_[i]).squaredNorm(); if (r > aabb_radius) aabb_radius = r; } @@ -841,7 +840,7 @@ template int BVHModel::memUsage(const bool msg) const { unsigned int mem_bv_list = (unsigned int)sizeof(BV) * num_bvs; unsigned int mem_tri_list = (unsigned int)sizeof(Triangle) * num_tris; - unsigned int mem_vertex_list = (unsigned int)sizeof(Vec3f) * num_vertices; + unsigned int mem_vertex_list = (unsigned int)sizeof(Vec3s) * num_vertices; unsigned int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list + (unsigned int)sizeof(BVHModel); @@ -858,7 +857,7 @@ int BVHModel::memUsage(const bool msg) const { template int BVHModel::buildTree() { // set BVFitter - Vec3f* vertices_ = vertices.get() ? vertices->data() : NULL; + Vec3s* vertices_ = vertices.get() ? vertices->data() : NULL; Triangle* tri_indices_ = tri_indices.get() ? tri_indices->data() : NULL; bv_fitter->set(vertices_, tri_indices_, getModelType()); // set SplitRule @@ -912,17 +911,17 @@ int BVHModel::recursiveBuildTree(int bv_id, unsigned int first_primitive, num_bvs += 2; unsigned int c1 = 0; - const std::vector& vertices_ = *vertices; + const std::vector& vertices_ = *vertices; const std::vector& tri_indices_ = *tri_indices; for (unsigned int i = 0; i < num_primitives; ++i) { - Vec3f p; + Vec3s p; if (type == BVH_MODEL_POINTCLOUD) p = vertices_[cur_primitive_indices[i]]; else if (type == BVH_MODEL_TRIANGLES) { const Triangle& t = tri_indices_[cur_primitive_indices[i]]; - const Vec3f& p1 = vertices_[t[0]]; - const Vec3f& p2 = vertices_[t[1]]; - const Vec3f& p3 = vertices_[t[2]]; + const Vec3s& p1 = vertices_[t[0]]; + const Vec3s& p2 = vertices_[t[1]]; + const Vec3s& p3 = vertices_[t[2]]; p = (p1 + p2 + p3) / 3.; } else { @@ -990,7 +989,7 @@ int BVHModel::recursiveRefitTree_bottomup(int bv_id) { BV bv; if (prev_vertices.get()) { - Vec3f v[2]; + Vec3s v[2]; v[0] = (*prev_vertices)[static_cast(primitive_id)]; v[1] = (*vertices)[static_cast(primitive_id)]; fit(v, 2, bv); @@ -1004,7 +1003,7 @@ int BVHModel::recursiveRefitTree_bottomup(int bv_id) { (*tri_indices)[static_cast(primitive_id)]; if (prev_vertices.get()) { - Vec3f v[6]; + Vec3s v[6]; for (Triangle::index_type i = 0; i < 3; ++i) { v[i] = (*prev_vertices)[triangle[i]]; v[i + 3] = (*vertices)[triangle[i]]; @@ -1016,7 +1015,7 @@ int BVHModel::recursiveRefitTree_bottomup(int bv_id) { // unsigned int* cur_primitive_indices = primitive_indices + // bvnode->first_primitive; bv = bv_fitter->fit(cur_primitive_indices, // bvnode->num_primitives); - Vec3f v[3]; + Vec3s v[3]; for (int i = 0; i < 3; ++i) { v[i] = (*vertices)[triangle[(Triangle::index_type)i]]; } @@ -1045,8 +1044,8 @@ int BVHModel::recursiveRefitTree_bottomup(int bv_id) { template int BVHModel::refitTree_topdown() { - Vec3f* vertices_ = vertices.get() ? vertices->data() : NULL; - Vec3f* prev_vertices_ = prev_vertices.get() ? prev_vertices->data() : NULL; + Vec3s* vertices_ = vertices.get() ? vertices->data() : NULL; + Vec3s* prev_vertices_ = prev_vertices.get() ? prev_vertices->data() : NULL; Triangle* tri_indices_ = tri_indices.get() ? tri_indices->data() : NULL; bv_fitter->set(vertices_, prev_vertices_, tri_indices_, getModelType()); BVNode* bvs_ = bvs->data(); @@ -1063,8 +1062,8 @@ int BVHModel::refitTree_topdown() { } template <> -void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, - const Vec3f& parent_c) { +void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, + const Vec3s& parent_c) { bv_node_vector_t& bvs_ = *bvs; OBB& obb = bvs_[static_cast(bv_id)].bv; if (!bvs_[static_cast(bv_id)].isLeaf()) { @@ -1079,13 +1078,13 @@ void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, // obb.axes = parent_axes.transpose() * obb.axes; obb.axes.applyOnTheLeft(parent_axes.transpose()); - Vec3f t(obb.To - parent_c); + Vec3s t(obb.To - parent_c); obb.To.noalias() = parent_axes.transpose() * t; } template <> -void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, - const Vec3f& parent_c) { +void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, + const Vec3s& parent_c) { bv_node_vector_t& bvs_ = *bvs; RSS& rss = bvs_[static_cast(bv_id)].bv; if (!bvs_[static_cast(bv_id)].isLeaf()) { @@ -1100,14 +1099,14 @@ void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, // rss.axes = parent_axes.transpose() * rss.axes; rss.axes.applyOnTheLeft(parent_axes.transpose()); - Vec3f t(rss.Tr - parent_c); + Vec3s t(rss.Tr - parent_c); rss.Tr.noalias() = parent_axes.transpose() * t; } template <> void BVHModel::makeParentRelativeRecurse(int bv_id, - Matrix3f& parent_axes, - const Vec3f& parent_c) { + Matrix3s& parent_axes, + const Vec3s& parent_c) { bv_node_vector_t& bvs_ = *bvs; OBB& obb = bvs_[static_cast(bv_id)].bv.obb; RSS& rss = bvs_[static_cast(bv_id)].bv.rss; @@ -1123,7 +1122,7 @@ void BVHModel::makeParentRelativeRecurse(int bv_id, rss.axes.noalias() = parent_axes.transpose() * obb.axes; obb.axes = rss.axes; - Vec3f t(obb.To - parent_c); + Vec3s t(obb.To - parent_c); obb.To.noalias() = parent_axes.transpose() * t; rss.Tr = obb.To; } @@ -1177,6 +1176,4 @@ template class BVHModel; template class BVHModel; template class BVHModel; -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp index 6f644e307..d3a74960c 100644 --- a/src/BVH/BVH_utility.cpp +++ b/src/BVH/BVH_utility.cpp @@ -35,23 +35,22 @@ /** \author Jia Pan */ -#include -#include -#include -#include +#include "coal/BVH/BVH_utility.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/internal/shape_shape_func.h" -namespace hpp { -namespace fcl { +namespace coal { namespace details { template -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, +BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& _aabb) { assert(model.getModelType() == BVH_MODEL_TRIANGLES); - const Matrix3f& q = pose.getRotation(); + const Matrix3s& q = pose.getRotation(); AABB aabb = translate(_aabb, -pose.getTranslation()); - Transform3f box_pose; + Transform3s box_pose; Box box; constructBox(_aabb, box, box_pose); box_pose = pose.inverseTimes(box_pose); @@ -65,7 +64,7 @@ BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, std::vector keep_vertex(model.num_vertices, false); std::vector keep_tri(model.num_tris, false); unsigned int ntri = 0; - const std::vector& model_vertices_ = *(model.vertices); + const std::vector& model_vertices_ = *(model.vertices); const std::vector& model_tri_indices_ = *(model.tri_indices); for (unsigned int i = 0; i < model.num_tris; ++i) { const Triangle& t = model_tri_indices_[i]; @@ -89,8 +88,8 @@ BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, const DistanceRequest distanceRequest(enable_nearest_points, compute_penetration); DistanceResult distanceResult; - const FCL_REAL distance = ShapeShapeDistance( - &box, box_pose, &tri, Transform3f(), &gjk, distanceRequest, + const CoalScalar distance = ShapeShapeDistance( + &box, box_pose, &tri, Transform3s(), &gjk, distanceRequest, distanceResult); bool is_collision = (distance <= gjk.getDistancePrecision(compute_penetration)); @@ -112,7 +111,7 @@ BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, new_model->beginModel(ntri, std::min(ntri * 3, model.num_vertices)); std::vector idxConversion(model.num_vertices); assert(new_model->num_vertices == 0); - std::vector& new_model_vertices_ = *(new_model->vertices); + std::vector& new_model_vertices_ = *(new_model->vertices); for (unsigned int i = 0; i < keep_vertex.size(); ++i) { if (keep_vertex[i]) { idxConversion[i] = new_model->num_vertices; @@ -140,58 +139,58 @@ BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, } // namespace details template <> -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, +BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, +BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, +BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, +BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, const AABB& aabb) { + const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> BVHModel >* BVHExtract(const BVHModel >& model, - const Transform3f& pose, const AABB& aabb) { + const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> BVHModel >* BVHExtract(const BVHModel >& model, - const Transform3f& pose, const AABB& aabb) { + const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> BVHModel >* BVHExtract(const BVHModel >& model, - const Transform3f& pose, const AABB& aabb) { + const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } -void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, - unsigned int n, Matrix3f& M) { - Vec3f S1(Vec3f::Zero()); - Vec3f S2[3] = {Vec3f::Zero(), Vec3f::Zero(), Vec3f::Zero()}; +void getCovariance(Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, + unsigned int n, Matrix3s& M) { + Vec3s S1(Vec3s::Zero()); + Vec3s S2[3] = {Vec3s::Zero(), Vec3s::Zero(), Vec3s::Zero()}; if (ts) { for (unsigned int i = 0; i < n; ++i) { const Triangle& t = (indices) ? ts[indices[i]] : ts[i]; - const Vec3f& p1 = ps[t[0]]; - const Vec3f& p2 = ps[t[1]]; - const Vec3f& p3 = ps[t[2]]; + const Vec3s& p1 = ps[t[0]]; + const Vec3s& p2 = ps[t[1]]; + const Vec3s& p3 = ps[t[2]]; S1[0] += (p1[0] + p2[0] + p3[0]); S1[1] += (p1[1] + p2[1] + p3[1]); @@ -204,9 +203,9 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]); if (ps2) { - const Vec3f& p1 = ps2[t[0]]; - const Vec3f& p2 = ps2[t[1]]; - const Vec3f& p3 = ps2[t[2]]; + const Vec3s& p1 = ps2[t[0]]; + const Vec3s& p2 = ps2[t[1]]; + const Vec3s& p3 = ps2[t[2]]; S1[0] += (p1[0] + p2[0] + p3[0]); S1[1] += (p1[1] + p2[1] + p3[1]); @@ -222,7 +221,7 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, } } else { for (unsigned int i = 0; i < n; ++i) { - const Vec3f& p = (indices) ? ps[indices[i]] : ps[i]; + const Vec3s& p = (indices) ? ps[indices[i]] : ps[i]; S1 += p; S2[0][0] += (p[0] * p[0]); S2[1][1] += (p[1] * p[1]); @@ -233,7 +232,7 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, if (ps2) // another frame { - const Vec3f& p = (indices) ? ps2[indices[i]] : ps2[i]; + const Vec3s& p = (indices) ? ps2[indices[i]] : ps2[i]; S1 += p; S2[0][0] += (p[0] * p[0]); S2[1][1] += (p[1] * p[1]); @@ -261,16 +260,16 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, /** @brief Compute the RSS bounding volume parameters: radius, rectangle size * and the origin. The bounding volume axes are known. */ -void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, +void getRadiusAndOriginAndRectangleSize(Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, unsigned int n, - const Matrix3f& axes, Vec3f& origin, - FCL_REAL l[2], FCL_REAL& r) { + const Matrix3s& axes, Vec3s& origin, + CoalScalar l[2], CoalScalar& r) { bool indirect_index = true; if (!indices) indirect_index = false; unsigned int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; - FCL_REAL(*P)[3] = new FCL_REAL[size_P][3]; + CoalScalar(*P)[3] = new CoalScalar[size_P][3]; int P_id = 0; @@ -281,8 +280,8 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; - const Vec3f& p = ps[point_id]; - Vec3f v(p[0], p[1], p[2]); + const Vec3s& p = ps[point_id]; + Vec3s v(p[0], p[1], p[2]); P[P_id][0] = axes.col(0).dot(v); P[P_id][1] = axes.col(1).dot(v); P[P_id][2] = axes.col(2).dot(v); @@ -292,9 +291,9 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, if (ps2) { for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; - const Vec3f& p = ps2[point_id]; + const Vec3s& p = ps2[point_id]; // FIXME Is this right ????? - Vec3f v(p[0], p[1], p[2]); + Vec3s v(p[0], p[1], p[2]); P[P_id][0] = axes.col(0).dot(v); P[P_id][1] = axes.col(0).dot(v); P[P_id][2] = axes.col(1).dot(v); @@ -306,15 +305,15 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; - const Vec3f& p = ps[index]; - Vec3f v(p[0], p[1], p[2]); + const Vec3s& p = ps[index]; + Vec3s v(p[0], p[1], p[2]); P[P_id][0] = axes.col(0).dot(v); P[P_id][1] = axes.col(1).dot(v); P[P_id][2] = axes.col(2).dot(v); P_id++; if (ps2) { - const Vec3f& v = ps2[index]; + const Vec3s& v = ps2[index]; P[P_id][0] = axes.col(0).dot(v); P[P_id][1] = axes.col(1).dot(v); P[P_id][2] = axes.col(2).dot(v); @@ -323,34 +322,34 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, } } - FCL_REAL minx, maxx, miny, maxy, minz, maxz; + CoalScalar minx, maxx, miny, maxy, minz, maxz; - FCL_REAL cz, radsqr; + CoalScalar cz, radsqr; minz = maxz = P[0][2]; for (unsigned int i = 1; i < size_P; ++i) { - FCL_REAL z_value = P[i][2]; + CoalScalar z_value = P[i][2]; if (z_value < minz) minz = z_value; else if (z_value > maxz) maxz = z_value; } - r = (FCL_REAL)0.5 * (maxz - minz); + r = (CoalScalar)0.5 * (maxz - minz); radsqr = r * r; - cz = (FCL_REAL)0.5 * (maxz + minz); + cz = (CoalScalar)0.5 * (maxz + minz); // compute an initial norm of rectangle along x direction // find minx and maxx as starting points unsigned int minindex = 0, maxindex = 0; - FCL_REAL mintmp, maxtmp; + CoalScalar mintmp, maxtmp; mintmp = maxtmp = P[0][0]; for (unsigned int i = 1; i < size_P; ++i) { - FCL_REAL x_value = P[i][0]; + CoalScalar x_value = P[i][0]; if (x_value < mintmp) { minindex = i; mintmp = x_value; @@ -360,22 +359,22 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, } } - FCL_REAL x, dz; + CoalScalar x, dz; dz = P[minindex][2] - cz; - minx = P[minindex][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); + minx = P[minindex][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); dz = P[maxindex][2] - cz; - maxx = P[maxindex][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); + maxx = P[maxindex][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); // grow minx/maxx for (unsigned int i = 0; i < size_P; ++i) { if (P[i][0] < minx) { dz = P[i][2] - cz; - x = P[i][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); + x = P[i][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); if (x < minx) minx = x; } else if (P[i][0] > maxx) { dz = P[i][2] - cz; - x = P[i][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); + x = P[i][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); if (x > maxx) maxx = x; } } @@ -387,7 +386,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, minindex = maxindex = 0; mintmp = maxtmp = P[0][1]; for (unsigned int i = 1; i < size_P; ++i) { - FCL_REAL y_value = P[i][1]; + CoalScalar y_value = P[i][1]; if (y_value < mintmp) { minindex = i; mintmp = y_value; @@ -397,30 +396,30 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, } } - FCL_REAL y; + CoalScalar y; dz = P[minindex][2] - cz; - miny = P[minindex][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); + miny = P[minindex][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); dz = P[maxindex][2] - cz; - maxy = P[maxindex][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); + maxy = P[maxindex][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); // grow miny/maxy for (unsigned int i = 0; i < size_P; ++i) { if (P[i][1] < miny) { dz = P[i][2] - cz; - y = P[i][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); + y = P[i][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); if (y < miny) miny = y; } else if (P[i][1] > maxy) { dz = P[i][2] - cz; - y = P[i][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); + y = P[i][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); if (y > maxy) maxy = y; } } // corners may have some points which are not covered - grow lengths if // necessary quite conservative (can be improved) - FCL_REAL dx, dy, u, t; - FCL_REAL a = std::sqrt((FCL_REAL)0.5); + CoalScalar dx, dy, u, t; + CoalScalar a = std::sqrt((CoalScalar)0.5); for (unsigned int i = 0; i < size_P; ++i) { if (P[i][0] > maxx) { if (P[i][1] > maxy) { @@ -429,7 +428,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, u = dx * a + dy * a; t = (a * u - dx) * (a * u - dx) + (a * u - dy) * (a * u - dy) + (cz - P[i][2]) * (cz - P[i][2]); - u = u - std::sqrt(std::max(radsqr - t, 0)); + u = u - std::sqrt(std::max(radsqr - t, 0)); if (u > 0) { maxx += u * a; maxy += u * a; @@ -440,7 +439,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, u = dx * a - dy * a; t = (a * u - dx) * (a * u - dx) + (-a * u - dy) * (-a * u - dy) + (cz - P[i][2]) * (cz - P[i][2]); - u = u - std::sqrt(std::max(radsqr - t, 0)); + u = u - std::sqrt(std::max(radsqr - t, 0)); if (u > 0) { maxx += u * a; miny -= u * a; @@ -453,7 +452,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, u = dy * a - dx * a; t = (-a * u - dx) * (-a * u - dx) + (a * u - dy) * (a * u - dy) + (cz - P[i][2]) * (cz - P[i][2]); - u = u - std::sqrt(std::max(radsqr - t, 0)); + u = u - std::sqrt(std::max(radsqr - t, 0)); if (u > 0) { minx -= u * a; maxy += u * a; @@ -464,7 +463,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, u = -dx * a - dy * a; t = (-a * u - dx) * (-a * u - dx) + (-a * u - dy) * (-a * u - dy) + (cz - P[i][2]) * (cz - P[i][2]); - u = u - std::sqrt(std::max(radsqr - t, 0)); + u = u - std::sqrt(std::max(radsqr - t, 0)); if (u > 0) { minx -= u * a; miny -= u * a; @@ -473,10 +472,10 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, } } - origin.noalias() = axes * Vec3f(minx, miny, cz); + origin.noalias() = axes * Vec3s(minx, miny, cz); - l[0] = std::max(maxx - minx, 0); - l[1] = std::max(maxy - miny, 0); + l[0] = std::max(maxx - minx, 0); + l[1] = std::max(maxy - miny, 0); delete[] P; } @@ -484,23 +483,23 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, /** @brief Compute the bounding volume extent and center for a set or subset of * points. The bounding volume axes are known. */ -static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, +static inline void getExtentAndCenter_pointcloud(Vec3s* ps, Vec3s* ps2, unsigned int* indices, - unsigned int n, Matrix3f& axes, - Vec3f& center, Vec3f& extent) { + unsigned int n, Matrix3s& axes, + Vec3s& center, Vec3s& extent) { bool indirect_index = true; if (!indices) indirect_index = false; - FCL_REAL real_max = (std::numeric_limits::max)(); + CoalScalar real_max = (std::numeric_limits::max)(); - Vec3f min_coord(real_max, real_max, real_max); - Vec3f max_coord(-real_max, -real_max, -real_max); + Vec3s min_coord(real_max, real_max, real_max); + Vec3s max_coord(-real_max, -real_max, -real_max); for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; - const Vec3f& p = ps[index]; - Vec3f proj(axes.transpose() * p); + const Vec3s& p = ps[index]; + Vec3s proj(axes.transpose() * p); for (int j = 0; j < 3; ++j) { if (proj[j] > max_coord[j]) max_coord[j] = proj[j]; @@ -508,7 +507,7 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, } if (ps2) { - const Vec3f& v = ps2[index]; + const Vec3s& v = ps2[index]; proj.noalias() = axes.transpose() * v; for (int j = 0; j < 3; ++j) { @@ -526,17 +525,17 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, /** @brief Compute the bounding volume extent and center for a set or subset of * points. The bounding volume axes are known. */ -static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, +static inline void getExtentAndCenter_mesh(Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, - unsigned int n, Matrix3f& axes, - Vec3f& center, Vec3f& extent) { + unsigned int n, Matrix3s& axes, + Vec3s& center, Vec3s& extent) { bool indirect_index = true; if (!indices) indirect_index = false; - FCL_REAL real_max = (std::numeric_limits::max)(); + CoalScalar real_max = (std::numeric_limits::max)(); - Vec3f min_coord(real_max, real_max, real_max); - Vec3f max_coord(-real_max, -real_max, -real_max); + Vec3s min_coord(real_max, real_max, real_max); + Vec3s max_coord(-real_max, -real_max, -real_max); for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; @@ -544,8 +543,8 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; - const Vec3f& p = ps[point_id]; - Vec3f proj(axes.transpose() * p); + const Vec3s& p = ps[point_id]; + Vec3s proj(axes.transpose() * p); for (int k = 0; k < 3; ++k) { if (proj[k] > max_coord[k]) max_coord[k] = proj[k]; @@ -556,8 +555,8 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, if (ps2) { for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; - const Vec3f& p = ps2[point_id]; - Vec3f proj(axes.transpose() * p); + const Vec3s& p = ps2[point_id]; + Vec3s proj(axes.transpose() * p); for (int k = 0; k < 3; ++k) { if (proj[k] > max_coord[k]) max_coord[k] = proj[k]; @@ -567,62 +566,63 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, } } - Vec3f o((max_coord + min_coord) / 2); + Vec3s o((max_coord + min_coord) / 2); center.noalias() = axes * o; extent.noalias() = (max_coord - min_coord) / 2; } -void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, - unsigned int* indices, unsigned int n, Matrix3f& axes, - Vec3f& center, Vec3f& extent) { +void getExtentAndCenter(Vec3s* ps, Vec3s* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, Matrix3s& axes, + Vec3s& center, Vec3s& extent) { if (ts) getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axes, center, extent); else getExtentAndCenter_pointcloud(ps, ps2, indices, n, axes, center, extent); } -void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, - Vec3f& center, FCL_REAL& radius) { - Vec3f e1 = a - c; - Vec3f e2 = b - c; - FCL_REAL e1_len2 = e1.squaredNorm(); - FCL_REAL e2_len2 = e2.squaredNorm(); - Vec3f e3 = e1.cross(e2); - FCL_REAL e3_len2 = e3.squaredNorm(); +void circumCircleComputation(const Vec3s& a, const Vec3s& b, const Vec3s& c, + Vec3s& center, CoalScalar& radius) { + Vec3s e1 = a - c; + Vec3s e2 = b - c; + CoalScalar e1_len2 = e1.squaredNorm(); + CoalScalar e2_len2 = e2.squaredNorm(); + Vec3s e3 = e1.cross(e2); + CoalScalar e3_len2 = e3.squaredNorm(); radius = e1_len2 * e2_len2 * (e1 - e2).squaredNorm() / e3_len2; radius = std::sqrt(radius) * 0.5; center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c; } -static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, - unsigned int* indices, - unsigned int n, - const Vec3f& query) { +static inline CoalScalar maximumDistance_mesh(Vec3s* ps, Vec3s* ps2, + Triangle* ts, + unsigned int* indices, + unsigned int n, + const Vec3s& query) { bool indirect_index = true; if (!indices) indirect_index = false; - FCL_REAL maxD = 0; + CoalScalar maxD = 0; for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; const Triangle& t = ts[index]; for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; - const Vec3f& p = ps[point_id]; + const Vec3s& p = ps[point_id]; - FCL_REAL d = (p - query).squaredNorm(); + CoalScalar d = (p - query).squaredNorm(); if (d > maxD) maxD = d; } if (ps2) { for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; - const Vec3f& p = ps2[point_id]; + const Vec3s& p = ps2[point_id]; - FCL_REAL d = (p - query).squaredNorm(); + CoalScalar d = (p - query).squaredNorm(); if (d > maxD) maxD = d; } } @@ -631,24 +631,24 @@ static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, return std::sqrt(maxD); } -static inline FCL_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, - unsigned int* indices, - unsigned int n, - const Vec3f& query) { +static inline CoalScalar maximumDistance_pointcloud(Vec3s* ps, Vec3s* ps2, + unsigned int* indices, + unsigned int n, + const Vec3s& query) { bool indirect_index = true; if (!indices) indirect_index = false; - FCL_REAL maxD = 0; + CoalScalar maxD = 0; for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; - const Vec3f& p = ps[index]; - FCL_REAL d = (p - query).squaredNorm(); + const Vec3s& p = ps[index]; + CoalScalar d = (p - query).squaredNorm(); if (d > maxD) maxD = d; if (ps2) { - const Vec3f& v = ps2[index]; - FCL_REAL d = (v - query).squaredNorm(); + const Vec3s& v = ps2[index]; + CoalScalar d = (v - query).squaredNorm(); if (d > maxD) maxD = d; } } @@ -656,15 +656,13 @@ static inline FCL_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, return std::sqrt(maxD); } -FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, - unsigned int* indices, unsigned int n, - const Vec3f& query) { +CoalScalar maximumDistance(Vec3s* ps, Vec3s* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, + const Vec3s& query) { if (ts) return maximumDistance_mesh(ps, ps2, ts, indices, n, query); else return maximumDistance_pointcloud(ps, ps2, indices, n, query); } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp index 730447a38..02a8bc8c9 100644 --- a/src/BVH/BV_fitter.cpp +++ b/src/BVH/BV_fitter.cpp @@ -35,20 +35,20 @@ /** \author Jia Pan */ -#include -#include +#include "coal/internal/BV_fitter.h" +#include "coal/BVH/BVH_utility.h" +#include "coal/internal/tools.h" + #include -#include -namespace hpp { -namespace fcl { +namespace coal { static const double kIOS_RATIO = 1.5; static const double invSinA = 2; static const double cosA = sqrt(3.0) / 2.0; -static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::Scalar eigenS[3], - Matrix3f& axes) { +static inline void axisFromEigen(Vec3s eigenV[3], CoalScalar eigenS[3], + Matrix3s& axes) { int min, mid, max; if (eigenS[0] > eigenS[1]) { max = 0; @@ -77,17 +77,17 @@ static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::Scalar eigenS[3], namespace OBB_fit_functions { -void fit1(Vec3f* ps, OBB& bv) { +void fit1(Vec3s* ps, OBB& bv) { bv.To.noalias() = ps[0]; bv.axes.setIdentity(); bv.extent.setZero(); } -void fit2(Vec3f* ps, OBB& bv) { - const Vec3f& p1 = ps[0]; - const Vec3f& p2 = ps[1]; - Vec3f p1p2 = p1 - p2; - FCL_REAL len_p1p2 = p1p2.norm(); +void fit2(Vec3s* ps, OBB& bv) { + const Vec3s& p1 = ps[0]; + const Vec3s& p2 = ps[1]; + Vec3s p1p2 = p1 - p2; + CoalScalar len_p1p2 = p1p2.norm(); p1p2.normalize(); bv.axes.col(0).noalias() = p1p2; @@ -97,15 +97,15 @@ void fit2(Vec3f* ps, OBB& bv) { bv.To.noalias() = (p1 + p2) / 2; } -void fit3(Vec3f* ps, OBB& bv) { - const Vec3f& p1 = ps[0]; - const Vec3f& p2 = ps[1]; - const Vec3f& p3 = ps[2]; - Vec3f e[3]; +void fit3(Vec3s* ps, OBB& bv) { + const Vec3s& p1 = ps[0]; + const Vec3s& p2 = ps[1]; + const Vec3s& p3 = ps[2]; + Vec3s e[3]; e[0] = p1 - p2; e[1] = p2 - p3; e[2] = p3 - p1; - FCL_REAL len[3]; + CoalScalar len[3]; len[0] = e[0].squaredNorm(); len[1] = e[1].squaredNorm(); len[2] = e[2].squaredNorm(); @@ -121,17 +121,17 @@ void fit3(Vec3f* ps, OBB& bv) { getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.axes, bv.To, bv.extent); } -void fit6(Vec3f* ps, OBB& bv) { +void fit6(Vec3s* ps, OBB& bv) { OBB bv1, bv2; fit3(ps, bv1); fit3(ps + 3, bv2); bv = bv1 + bv2; } -void fitn(Vec3f* ps, unsigned int n, OBB& bv) { - Matrix3f M; - Vec3f E[3]; - Matrix3f::Scalar s[3] = {0, 0, 0}; // three eigen values +void fitn(Vec3s* ps, unsigned int n, OBB& bv) { + Matrix3s M; + Vec3s E[3]; + CoalScalar s[3] = {0, 0, 0}; // three eigen values getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); @@ -144,7 +144,7 @@ void fitn(Vec3f* ps, unsigned int n, OBB& bv) { } // namespace OBB_fit_functions namespace RSS_fit_functions { -void fit1(Vec3f* ps, RSS& bv) { +void fit1(Vec3s* ps, RSS& bv) { bv.Tr.noalias() = ps[0]; bv.axes.setIdentity(); bv.length[0] = 0; @@ -152,11 +152,11 @@ void fit1(Vec3f* ps, RSS& bv) { bv.radius = 0; } -void fit2(Vec3f* ps, RSS& bv) { - const Vec3f& p1 = ps[0]; - const Vec3f& p2 = ps[1]; +void fit2(Vec3s* ps, RSS& bv) { + const Vec3s& p1 = ps[0]; + const Vec3s& p2 = ps[1]; bv.axes.col(0).noalias() = p1 - p2; - FCL_REAL len_p1p2 = bv.axes.col(0).norm(); + CoalScalar len_p1p2 = bv.axes.col(0).norm(); bv.axes.col(0) /= len_p1p2; generateCoordinateSystem(bv.axes.col(0), bv.axes.col(1), bv.axes.col(2)); @@ -167,15 +167,15 @@ void fit2(Vec3f* ps, RSS& bv) { bv.radius = 0; } -void fit3(Vec3f* ps, RSS& bv) { - const Vec3f& p1 = ps[0]; - const Vec3f& p2 = ps[1]; - const Vec3f& p3 = ps[2]; - Vec3f e[3]; +void fit3(Vec3s* ps, RSS& bv) { + const Vec3s& p1 = ps[0]; + const Vec3s& p2 = ps[1]; + const Vec3s& p3 = ps[2]; + Vec3s e[3]; e[0] = p1 - p2; e[1] = p2 - p3; e[2] = p3 - p1; - FCL_REAL len[3]; + CoalScalar len[3]; len[0] = e[0].squaredNorm(); len[1] = e[1].squaredNorm(); len[2] = e[2].squaredNorm(); @@ -192,17 +192,17 @@ void fit3(Vec3f* ps, RSS& bv) { bv.length, bv.radius); } -void fit6(Vec3f* ps, RSS& bv) { +void fit6(Vec3s* ps, RSS& bv) { RSS bv1, bv2; fit3(ps, bv1); fit3(ps + 3, bv2); bv = bv1 + bv2; } -void fitn(Vec3f* ps, unsigned int n, RSS& bv) { - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors - Matrix3f::Scalar s[3] = {0, 0, 0}; +void fitn(Vec3s* ps, unsigned int n, RSS& bv) { + Matrix3s M; // row first matrix + Vec3s E[3]; // row first eigen-vectors + CoalScalar s[3] = {0, 0, 0}; getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); @@ -217,7 +217,7 @@ void fitn(Vec3f* ps, unsigned int n, RSS& bv) { namespace kIOS_fit_functions { -void fit1(Vec3f* ps, kIOS& bv) { +void fit1(Vec3s* ps, kIOS& bv) { bv.num_spheres = 1; bv.spheres[0].o.noalias() = ps[0]; bv.spheres[0].r = 0; @@ -227,31 +227,31 @@ void fit1(Vec3f* ps, kIOS& bv) { bv.obb.To.noalias() = ps[0]; } -void fit2(Vec3f* ps, kIOS& bv) { +void fit2(Vec3s* ps, kIOS& bv) { bv.num_spheres = 5; - const Vec3f& p1 = ps[0]; - const Vec3f& p2 = ps[1]; - Vec3f p1p2 = p1 - p2; - FCL_REAL len_p1p2 = p1p2.norm(); + const Vec3s& p1 = ps[0]; + const Vec3s& p2 = ps[1]; + Vec3s p1p2 = p1 - p2; + CoalScalar len_p1p2 = p1p2.norm(); p1p2.normalize(); - Matrix3f& axes = bv.obb.axes; + Matrix3s& axes = bv.obb.axes; axes.col(0).noalias() = p1p2; generateCoordinateSystem(axes.col(0), axes.col(1), axes.col(2)); - FCL_REAL r0 = len_p1p2 * 0.5; + CoalScalar r0 = len_p1p2 * 0.5; bv.obb.extent << r0, 0, 0; bv.obb.To = (p1 + p2) * 0.5; bv.spheres[0].o = bv.obb.To; bv.spheres[0].r = r0; - FCL_REAL r1 = r0 * invSinA; - FCL_REAL r1cosA = r1 * cosA; + CoalScalar r1 = r0 * invSinA; + CoalScalar r1cosA = r1 * cosA; bv.spheres[1].r = r1; bv.spheres[2].r = r1; - Vec3f delta = axes.col(1) * r1cosA; + Vec3s delta = axes.col(1) * r1cosA; bv.spheres[1].o = bv.spheres[0].o - delta; bv.spheres[2].o = bv.spheres[0].o + delta; @@ -262,17 +262,17 @@ void fit2(Vec3f* ps, kIOS& bv) { bv.spheres[4].o = bv.spheres[0].o + delta; } -void fit3(Vec3f* ps, kIOS& bv) { +void fit3(Vec3s* ps, kIOS& bv) { bv.num_spheres = 3; - const Vec3f& p1 = ps[0]; - const Vec3f& p2 = ps[1]; - const Vec3f& p3 = ps[2]; - Vec3f e[3]; + const Vec3s& p1 = ps[0]; + const Vec3s& p2 = ps[1]; + const Vec3s& p3 = ps[2]; + Vec3s e[3]; e[0] = p1 - p2; e[1] = p2 - p3; e[2] = p3 - p1; - FCL_REAL len[3]; + CoalScalar len[3]; len[0] = e[0].squaredNorm(); len[1] = e[1].squaredNorm(); len[2] = e[2].squaredNorm(); @@ -289,15 +289,15 @@ void fit3(Vec3f* ps, kIOS& bv) { bv.obb.extent); // compute radius and center - FCL_REAL r0; - Vec3f center; + CoalScalar r0; + Vec3s center; circumCircleComputation(p1, p2, p3, center, r0); bv.spheres[0].o = center; bv.spheres[0].r = r0; - FCL_REAL r1 = r0 * invSinA; - Vec3f delta = bv.obb.axes.col(2) * (r1 * cosA); + CoalScalar r1 = r0 * invSinA; + Vec3s delta = bv.obb.axes.col(2) * (r1 * cosA); bv.spheres[1].r = r1; bv.spheres[1].o = center - delta; @@ -305,23 +305,23 @@ void fit3(Vec3f* ps, kIOS& bv) { bv.spheres[2].o = center + delta; } -void fitn(Vec3f* ps, unsigned int n, kIOS& bv) { - Matrix3f M; - Vec3f E[3]; - Matrix3f::Scalar s[3] = {0, 0, 0}; // three eigen values; +void fitn(Vec3s* ps, unsigned int n, kIOS& bv) { + Matrix3s M; + Vec3s E[3]; + CoalScalar s[3] = {0, 0, 0}; // three eigen values; getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); - Matrix3f& axes = bv.obb.axes; + Matrix3s& axes = bv.obb.axes; axisFromEigen(E, s, axes); getExtentAndCenter(ps, NULL, NULL, NULL, n, axes, bv.obb.To, bv.obb.extent); // get center and extension - const Vec3f& center = bv.obb.To; - const Vec3f& extent = bv.obb.extent; - FCL_REAL r0 = maximumDistance(ps, NULL, NULL, NULL, n, center); + const Vec3s& center = bv.obb.To; + const Vec3s& extent = bv.obb.extent; + CoalScalar r0 = maximumDistance(ps, NULL, NULL, NULL, n, center); // decide the k in kIOS if (extent[0] > kIOS_RATIO * extent[2]) { @@ -336,12 +336,12 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) { bv.spheres[0].r = r0; if (bv.num_spheres >= 3) { - FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; - Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]); + CoalScalar r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; + Vec3s delta = axes.col(2) * (r10 * cosA - extent[2]); bv.spheres[1].o = center - delta; bv.spheres[2].o = center + delta; - FCL_REAL r11 = 0, r12 = 0; + CoalScalar r11 = 0, r12 = 0; r11 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[1].o); r12 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[2].o); bv.spheres[1].o += axes.col(2) * (-r10 + r11); @@ -352,15 +352,15 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) { } if (bv.num_spheres >= 5) { - FCL_REAL r10 = bv.spheres[1].r; - Vec3f delta = + CoalScalar r10 = bv.spheres[1].r; + Vec3s delta = axes.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]); bv.spheres[3].o = bv.spheres[0].o - delta; bv.spheres[4].o = bv.spheres[0].o + delta; - FCL_REAL r21 = 0, r22 = 0; + CoalScalar r21 = 0, r22 = 0; r21 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[3].o); r22 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[4].o); @@ -375,22 +375,22 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) { } // namespace kIOS_fit_functions namespace OBBRSS_fit_functions { -void fit1(Vec3f* ps, OBBRSS& bv) { +void fit1(Vec3s* ps, OBBRSS& bv) { OBB_fit_functions::fit1(ps, bv.obb); RSS_fit_functions::fit1(ps, bv.rss); } -void fit2(Vec3f* ps, OBBRSS& bv) { +void fit2(Vec3s* ps, OBBRSS& bv) { OBB_fit_functions::fit2(ps, bv.obb); RSS_fit_functions::fit2(ps, bv.rss); } -void fit3(Vec3f* ps, OBBRSS& bv) { +void fit3(Vec3s* ps, OBBRSS& bv) { OBB_fit_functions::fit3(ps, bv.obb); RSS_fit_functions::fit3(ps, bv.rss); } -void fitn(Vec3f* ps, unsigned int n, OBBRSS& bv) { +void fitn(Vec3s* ps, unsigned int n, OBBRSS& bv) { OBB_fit_functions::fitn(ps, n, bv.obb); RSS_fit_functions::fitn(ps, n, bv.rss); } @@ -398,7 +398,7 @@ void fitn(Vec3f* ps, unsigned int n, OBBRSS& bv) { } // namespace OBBRSS_fit_functions template <> -void fit(Vec3f* ps, unsigned int n, OBB& bv) { +void fit(Vec3s* ps, unsigned int n, OBB& bv) { switch (n) { case 1: OBB_fit_functions::fit1(ps, bv); @@ -418,7 +418,7 @@ void fit(Vec3f* ps, unsigned int n, OBB& bv) { } template <> -void fit(Vec3f* ps, unsigned int n, RSS& bv) { +void fit(Vec3s* ps, unsigned int n, RSS& bv) { switch (n) { case 1: RSS_fit_functions::fit1(ps, bv); @@ -435,7 +435,7 @@ void fit(Vec3f* ps, unsigned int n, RSS& bv) { } template <> -void fit(Vec3f* ps, unsigned int n, kIOS& bv) { +void fit(Vec3s* ps, unsigned int n, kIOS& bv) { switch (n) { case 1: kIOS_fit_functions::fit1(ps, bv); @@ -452,7 +452,7 @@ void fit(Vec3f* ps, unsigned int n, kIOS& bv) { } template <> -void fit(Vec3f* ps, unsigned int n, OBBRSS& bv) { +void fit(Vec3s* ps, unsigned int n, OBBRSS& bv) { switch (n) { case 1: OBBRSS_fit_functions::fit1(ps, bv); @@ -469,7 +469,7 @@ void fit(Vec3f* ps, unsigned int n, OBBRSS& bv) { } template <> -void fit(Vec3f* ps, unsigned int n, AABB& bv) { +void fit(Vec3s* ps, unsigned int n, AABB& bv) { if (n <= 0) return; bv = AABB(ps[0]); for (unsigned int i = 1; i < n; ++i) { @@ -481,9 +481,9 @@ OBB BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { OBB bv; - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors - Matrix3f::Scalar s[3]; // three eigen values + Matrix3s M; // row first matrix + Vec3s E[3]; // row first eigen-vectors + CoalScalar s[3]; // three eigen values getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); @@ -501,9 +501,9 @@ OBB BVFitter::fit(unsigned int* primitive_indices, OBBRSS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { OBBRSS bv; - Matrix3f M; - Vec3f E[3]; - Matrix3f::Scalar s[3]; + Matrix3s M; + Vec3s E[3]; + CoalScalar s[3]; getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); @@ -515,9 +515,9 @@ OBBRSS BVFitter::fit(unsigned int* primitive_indices, getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.obb.axes, bv.obb.To, bv.obb.extent); - Vec3f origin; - FCL_REAL l[2]; - FCL_REAL r; + Vec3s origin; + CoalScalar l[2]; + CoalScalar r; getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.rss.axes, origin, l, r); @@ -534,9 +534,9 @@ RSS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { RSS bv; - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors - Matrix3f::Scalar s[3]; // three eigen values + Matrix3s M; // row first matrix + Vec3s E[3]; // row first eigen-vectors + CoalScalar s[3]; // three eigen values getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); eigen(M, s, E); @@ -544,9 +544,9 @@ RSS BVFitter::fit(unsigned int* primitive_indices, // set rss origin, rectangle size and radius - Vec3f origin; - FCL_REAL l[2]; - FCL_REAL r; + Vec3s origin; + CoalScalar l[2]; + CoalScalar r; getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axes, origin, l, r); @@ -563,25 +563,25 @@ kIOS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { kIOS bv; - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors - Matrix3f::Scalar s[3]; + Matrix3s M; // row first matrix + Vec3s E[3]; // row first eigen-vectors + CoalScalar s[3]; getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); eigen(M, s, E); - Matrix3f& axes = bv.obb.axes; + Matrix3s& axes = bv.obb.axes; axisFromEigen(E, s, axes); // get centers and extensions getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, axes, bv.obb.To, bv.obb.extent); - const Vec3f& center = bv.obb.To; - const Vec3f& extent = bv.obb.extent; - FCL_REAL r0 = maximumDistance(vertices, prev_vertices, tri_indices, - primitive_indices, num_primitives, center); + const Vec3s& center = bv.obb.To; + const Vec3s& extent = bv.obb.extent; + CoalScalar r0 = maximumDistance(vertices, prev_vertices, tri_indices, + primitive_indices, num_primitives, center); // decide k in kIOS if (extent[0] > kIOS_RATIO * extent[2]) { @@ -596,15 +596,15 @@ kIOS BVFitter::fit(unsigned int* primitive_indices, bv.spheres[0].r = r0; if (bv.num_spheres >= 3) { - FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; - Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]); + CoalScalar r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; + Vec3s delta = axes.col(2) * (r10 * cosA - extent[2]); bv.spheres[1].o = center - delta; bv.spheres[2].o = center + delta; - FCL_REAL r11 = + CoalScalar r11 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[1].o); - FCL_REAL r12 = + CoalScalar r12 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[2].o); @@ -616,15 +616,15 @@ kIOS BVFitter::fit(unsigned int* primitive_indices, } if (bv.num_spheres >= 5) { - FCL_REAL r10 = bv.spheres[1].r; - Vec3f delta = + CoalScalar r10 = bv.spheres[1].r; + Vec3s delta = axes.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]); bv.spheres[3].o = bv.spheres[0].o - delta; bv.spheres[4].o = bv.spheres[0].o + delta; - FCL_REAL r21 = 0, r22 = 0; + CoalScalar r21 = 0, r22 = 0; r21 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[3].o); r22 = maximumDistance(vertices, prev_vertices, tri_indices, @@ -679,6 +679,4 @@ AABB BVFitter::fit(unsigned int* primitive_indices, return bv; } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BVH/BV_splitter.cpp b/src/BVH/BV_splitter.cpp index 64dc26733..50d402429 100644 --- a/src/BVH/BV_splitter.cpp +++ b/src/BVH/BV_splitter.cpp @@ -35,27 +35,26 @@ /** \author Jia Pan */ -#include +#include "coal/internal/BV_splitter.h" -namespace hpp { -namespace fcl { +namespace coal { template -void computeSplitVector(const BV& bv, Vec3f& split_vector) { +void computeSplitVector(const BV& bv, Vec3s& split_vector) { split_vector = bv.axes.col(0); } template <> -void computeSplitVector(const kIOS& bv, Vec3f& split_vector) { +void computeSplitVector(const kIOS& bv, Vec3s& split_vector) { /* switch(bv.num_spheres) { case 1: - split_vector = Vec3f(1, 0, 0); + split_vector = Vec3s(1, 0, 0); break; case 3: { - Vec3f v[3]; + Vec3s v[3]; v[0] = bv.spheres[1].o - bv.spheres[0].o; v[0].normalize(); generateCoordinateSystem(v[0], v[1], v[2]); @@ -64,7 +63,7 @@ void computeSplitVector(const kIOS& bv, Vec3f& split_vector) { break; case 5: { - Vec3f v[2]; + Vec3s v[2]; v[0] = bv.spheres[1].o - bv.spheres[0].o; v[1] = bv.spheres[3].o - bv.spheres[0].o; split_vector = v[0].cross(v[1]); @@ -79,37 +78,38 @@ void computeSplitVector(const kIOS& bv, Vec3f& split_vector) { } template <> -void computeSplitVector(const OBBRSS& bv, Vec3f& split_vector) { +void computeSplitVector(const OBBRSS& bv, Vec3s& split_vector) { split_vector = bv.obb.axes.col(0); } template -void computeSplitValue_bvcenter(const BV& bv, FCL_REAL& split_value) { - Vec3f center = bv.center(); +void computeSplitValue_bvcenter(const BV& bv, CoalScalar& split_value) { + Vec3s center = bv.center(); split_value = center[0]; } template -void computeSplitValue_mean(const BV&, Vec3f* vertices, Triangle* triangles, +void computeSplitValue_mean(const BV&, Vec3s* vertices, Triangle* triangles, unsigned int* primitive_indices, unsigned int num_primitives, BVHModelType type, - const Vec3f& split_vector, FCL_REAL& split_value) { + const Vec3s& split_vector, + CoalScalar& split_value) { if (type == BVH_MODEL_TRIANGLES) { - Vec3f c(Vec3f::Zero()); + Vec3s c(Vec3s::Zero()); for (unsigned int i = 0; i < num_primitives; ++i) { const Triangle& t = triangles[primitive_indices[i]]; - const Vec3f& p1 = vertices[t[0]]; - const Vec3f& p2 = vertices[t[1]]; - const Vec3f& p3 = vertices[t[2]]; + const Vec3s& p1 = vertices[t[0]]; + const Vec3s& p2 = vertices[t[1]]; + const Vec3s& p3 = vertices[t[2]]; c += p1 + p2 + p3; } split_value = c.dot(split_vector) / (3 * num_primitives); } else if (type == BVH_MODEL_POINTCLOUD) { - FCL_REAL sum = 0; + CoalScalar sum = 0; for (unsigned int i = 0; i < num_primitives; ++i) { - const Vec3f& p = vertices[primitive_indices[i]]; + const Vec3s& p = vertices[primitive_indices[i]]; sum += p.dot(split_vector); } @@ -118,28 +118,28 @@ void computeSplitValue_mean(const BV&, Vec3f* vertices, Triangle* triangles, } template -void computeSplitValue_median(const BV&, Vec3f* vertices, Triangle* triangles, +void computeSplitValue_median(const BV&, Vec3s* vertices, Triangle* triangles, unsigned int* primitive_indices, unsigned int num_primitives, BVHModelType type, - const Vec3f& split_vector, - FCL_REAL& split_value) { - std::vector proj(num_primitives); + const Vec3s& split_vector, + CoalScalar& split_value) { + std::vector proj(num_primitives); if (type == BVH_MODEL_TRIANGLES) { for (unsigned int i = 0; i < num_primitives; ++i) { const Triangle& t = triangles[primitive_indices[i]]; - const Vec3f& p1 = vertices[t[0]]; - const Vec3f& p2 = vertices[t[1]]; - const Vec3f& p3 = vertices[t[2]]; - Vec3f centroid3(p1[0] + p2[0] + p3[0], p1[1] + p2[1] + p3[1], + const Vec3s& p1 = vertices[t[0]]; + const Vec3s& p2 = vertices[t[1]]; + const Vec3s& p3 = vertices[t[2]]; + Vec3s centroid3(p1[0] + p2[0] + p3[0], p1[1] + p2[1] + p3[1], p1[2] + p2[2] + p3[2]); proj[i] = centroid3.dot(split_vector) / 3; } } else if (type == BVH_MODEL_POINTCLOUD) { for (unsigned int i = 0; i < num_primitives; ++i) { - const Vec3f& p = vertices[primitive_indices[i]]; - Vec3f v(p[0], p[1], p[2]); + const Vec3s& p = vertices[primitive_indices[i]]; + Vec3s v(p[0], p[1], p[2]); proj[i] = v.dot(split_vector); } } @@ -259,23 +259,23 @@ void BVSplitter::computeRule_median(const OBBRSS& bv, } template <> -bool BVSplitter::apply(const Vec3f& q) const { - return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value; +bool BVSplitter::apply(const Vec3s& q) const { + return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value; } template <> -bool BVSplitter::apply(const Vec3f& q) const { - return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value; +bool BVSplitter::apply(const Vec3s& q) const { + return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value; } template <> -bool BVSplitter::apply(const Vec3f& q) const { - return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value; +bool BVSplitter::apply(const Vec3s& q) const { + return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value; } template <> -bool BVSplitter::apply(const Vec3f& q) const { - return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value; +bool BVSplitter::apply(const Vec3s& q) const { + return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value; } template class BVSplitter; @@ -283,6 +283,4 @@ template class BVSplitter; template class BVSplitter; template class BVSplitter; -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 74e7fb304..f1dfb892a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -110,11 +110,11 @@ set(${LIBRARY_NAME}_SOURCES serialization/serialization.cpp ) -if(HPP_FCL_HAS_OCTOMAP) +if(COAL_HAS_OCTOMAP) list(APPEND ${LIBRARY_NAME}_SOURCES octree.cpp) -endif(HPP_FCL_HAS_OCTOMAP) +endif(COAL_HAS_OCTOMAP) -if(HPP_FCL_HAS_QHULL AND NOT HPP_FCL_USE_SYSTEM_QHULL) +if(COAL_HAS_QHULL AND NOT COAL_USE_SYSTEM_QHULL) set( libqhullcpp_HEADERS ${Qhullcpp_PREFIX}/libqhullcpp/Coordinates.h @@ -179,14 +179,20 @@ SET(PROJECT_HEADERS_FULL_PATH) FOREACH(header ${${PROJECT_NAME}_HEADERS}) LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_SOURCE_DIR}/${header}) ENDFOREACH() -LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/hpp/fcl/config.hh) -LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/hpp/fcl/deprecated.hh) -LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/hpp/fcl/warning.hh) +LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/coal/config.hh) +LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/coal/deprecated.hh) +LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/coal/warning.hh) add_library(${LIBRARY_NAME} SHARED ${PROJECT_HEADERS_FULL_PATH} ${${LIBRARY_NAME}_SOURCES} ) +add_library(${LIBRARY_NAME}::${LIBRARY_NAME} ALIAS ${LIBRARY_NAME}) + +if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) + add_library(hpp-fcl ALIAS ${LIBRARY_NAME}) + add_library(hpp-fcl::hpp-fcl ALIAS ${LIBRARY_NAME}) +endif() if(UNIX) get_relative_rpath(${CMAKE_INSTALL_LIBDIR} ${PROJECT_NAME}_INSTALL_RPATH) @@ -213,10 +219,10 @@ TARGET_LINK_LIBRARIES(${LIBRARY_NAME} Boost::filesystem ) -if (HPP_FCL_ENABLE_LOGGING) +if (COAL_ENABLE_LOGGING) TARGET_LINK_LIBRARIES(${LIBRARY_NAME} PUBLIC Boost::log) # The compile flag `BOOST_LOG_DYN_LINK` is required here. - target_compile_definitions(${LIBRARY_NAME} PUBLIC HPP_FCL_ENABLE_LOGGING BOOST_LOG_DYN_LINK) + target_compile_definitions(${LIBRARY_NAME} PUBLIC COAL_ENABLE_LOGGING BOOST_LOG_DYN_LINK) endif() IF(WIN32) @@ -230,13 +236,17 @@ IF(WIN32) target_compile_definitions(${LIBRARY_NAME} PRIVATE _ENABLE_EXTENDED_ALIGNED_STORAGE) ENDIF(WIN32) -if (HPP_FCL_TURN_ASSERT_INTO_EXCEPTION) - target_compile_definitions(${LIBRARY_NAME} PUBLIC -DHPP_FCL_TURN_ASSERT_INTO_EXCEPTION) +if (COAL_TURN_ASSERT_INTO_EXCEPTION) + target_compile_definitions(${LIBRARY_NAME} PUBLIC -DCOAL_TURN_ASSERT_INTO_EXCEPTION) +endif() + +if (COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) + target_compile_definitions(${LIBRARY_NAME} PUBLIC -DCOAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) endif() -if(HPP_FCL_HAS_QHULL) - target_compile_definitions(${LIBRARY_NAME} PRIVATE -DHPP_FCL_HAS_QHULL) - if (HPP_FCL_USE_SYSTEM_QHULL) +if(COAL_HAS_QHULL) + target_compile_definitions(${LIBRARY_NAME} PRIVATE -DCOAL_HAS_QHULL) + if (COAL_USE_SYSTEM_QHULL) target_link_libraries(${LIBRARY_NAME} PRIVATE Qhull::qhull_r Qhull::qhullcpp) else() target_include_directories(${LIBRARY_NAME} SYSTEM PRIVATE @@ -252,6 +262,8 @@ MODERNIZE_TARGET_LINK_LIBRARIES(${PROJECT_NAME} SCOPE PUBLIC target_include_directories(${LIBRARY_NAME} PUBLIC $ + $ + $ ) IF(octomap_FOUND) @@ -260,8 +272,8 @@ IF(octomap_FOUND) LIBRARIES ${OCTOMAP_LIBRARIES} INCLUDE_DIRS ${OCTOMAP_INCLUDE_DIRS}) target_compile_definitions (${LIBRARY_NAME} PUBLIC - -DHPP_FCL_HAS_OCTOMAP - -DHPP_FCL_HAVE_OCTOMAP + -DCOAL_HAS_OCTOMAP + -DCOAL_HAVE_OCTOMAP -DOCTOMAP_MAJOR_VERSION=${OCTOMAP_MAJOR_VERSION} -DOCTOMAP_MINOR_VERSION=${OCTOMAP_MINOR_VERSION} -DOCTOMAP_PATCH_VERSION=${OCTOMAP_PATCH_VERSION}) diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp index bc51940cf..a39953835 100644 --- a/src/broadphase/broadphase_SSaP.cpp +++ b/src/broadphase/broadphase_SSaP.cpp @@ -35,10 +35,9 @@ /** @author Jia Pan */ -#include "hpp/fcl/broadphase/broadphase_SSaP.h" +#include "coal/broadphase/broadphase_SSaP.h" -namespace hpp { -namespace fcl { +namespace coal { /** @brief Functor sorting objects according to the AABB lower x bound */ struct SortByXLow { @@ -65,7 +64,7 @@ struct SortByZLow { }; /** @brief Dummy collision object with a point AABB */ -class HPP_FCL_DLLAPI DummyCollisionObject : public CollisionObject { +class COAL_DLLAPI DummyCollisionObject : public CollisionObject { public: DummyCollisionObject(const AABB& aabb_) : CollisionObject(shared_ptr()) { @@ -184,7 +183,7 @@ bool SSaPCollisionManager::checkDis( typename std::vector::const_iterator pos_start, typename std::vector::const_iterator pos_end, CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const { + CoalScalar& min_dist) const { while (pos_start < pos_end) { if (*pos_start != obj) // no distance between the same object { @@ -257,19 +256,19 @@ void SSaPCollisionManager::distance(CollisionObject* obj, callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); distance_(obj, callback, min_dist); } //============================================================================== bool SSaPCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const { + CoalScalar& min_dist) const { static const unsigned int CUTOFF = 100; - Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; - Vec3f dummy_vector = obj->getAABB().max_; - if (min_dist < (std::numeric_limits::max)()) - dummy_vector += Vec3f(min_dist, min_dist, min_dist); + Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; + Vec3s dummy_vector = obj->getAABB().max_; + if (min_dist < (std::numeric_limits::max)()) + dummy_vector += Vec3s(min_dist, min_dist, min_dist); typename std::vector::const_iterator pos_start1 = objs_x.begin(); @@ -285,7 +284,7 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj, objs_z.end(); int status = 1; - FCL_REAL old_min_distance; + CoalScalar old_min_distance; while (1) { old_min_distance = min_dist; @@ -329,20 +328,20 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj, if (dist_res) return true; if (status == 1) { - if (old_min_distance < (std::numeric_limits::max)()) + if (old_min_distance < (std::numeric_limits::max)()) break; else { // from infinity to a finite one, only need one additional loop // to check the possible missed ones to the right of the objs array if (min_dist < old_min_distance) { dummy_vector = - obj->getAABB().max_ + Vec3f(min_dist, min_dist, min_dist); + obj->getAABB().max_ + Vec3s(min_dist, min_dist, min_dist); status = 0; } else // need more loop { if (dummy_vector.isApprox( obj->getAABB().max_, - std::numeric_limits::epsilon() * 100)) + std::numeric_limits::epsilon() * 100)) dummy_vector = dummy_vector + delta; else dummy_vector = dummy_vector * 2 - obj->getAABB().max_; @@ -368,12 +367,12 @@ int SSaPCollisionManager::selectOptimalAxis( typename std::vector::const_iterator& it_beg, typename std::vector::const_iterator& it_end) { /// simple sweep and prune method - FCL_REAL delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - - (objs_x[0])->getAABB().min_[0]; - FCL_REAL delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - - (objs_y[0])->getAABB().min_[1]; - FCL_REAL delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - - (objs_z[0])->getAABB().min_[2]; + CoalScalar delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - + (objs_x[0])->getAABB().min_[0]; + CoalScalar delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - + (objs_y[0])->getAABB().min_[1]; + CoalScalar delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - + (objs_z[0])->getAABB().min_[2]; int axis = 0; if (delta_y > delta_x && delta_y > delta_z) @@ -454,7 +453,7 @@ void SSaPCollisionManager::distance(DistanceCallBackBase* callback) const { typename std::vector::const_iterator it, it_end; selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end); - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); for (; it != it_end; ++it) { if (distance_(*it, callback, min_dist)) return; } @@ -499,7 +498,7 @@ void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, return; } - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); typename std::vector::const_iterator it, end; if (this->size() < other_manager->size()) { for (it = objs_x.begin(), end = objs_x.end(); it != end; ++it) @@ -517,5 +516,4 @@ bool SSaPCollisionManager::empty() const { return objs_x.empty(); } //============================================================================== size_t SSaPCollisionManager::size() const { return objs_x.size(); } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp index 4da0c995a..b2b27f5fe 100644 --- a/src/broadphase/broadphase_SaP.cpp +++ b/src/broadphase/broadphase_SaP.cpp @@ -35,10 +35,9 @@ /** @author Jia Pan */ -#include "hpp/fcl/broadphase/broadphase_SaP.h" +#include "coal/broadphase/broadphase_SaP.h" -namespace hpp { -namespace fcl { +namespace coal { //============================================================================== void SaPCollisionManager::unregisterObject(CollisionObject* obj) { @@ -119,15 +118,15 @@ void SaPCollisionManager::registerObjects( obj_aabb_map[other_objs[i]] = sapaabb; } - FCL_REAL scale[3]; + CoalScalar scale[3]; for (int coord = 0; coord < 3; ++coord) { std::sort( endpoints.begin(), endpoints.end(), - std::bind(std::less(), - std::bind(static_cast( + std::bind(std::less(), + std::bind(static_cast( &EndPoint::getVal), std::placeholders::_1, coord), - std::bind(static_cast( + std::bind(static_cast( &EndPoint::getVal), std::placeholders::_2, coord))); @@ -204,7 +203,7 @@ void SaPCollisionManager::registerObject(CollisionObject* obj) { } else // otherwise, find the correct location in the list and insert { EndPoint* curr_lo = new_sap->lo; - FCL_REAL curr_lo_val = curr_lo->getVal()[coord]; + CoalScalar curr_lo_val = curr_lo->getVal()[coord]; while ((current->getVal()[coord] < curr_lo_val) && (current->next[coord] != nullptr)) current = current->next[coord]; @@ -232,7 +231,7 @@ void SaPCollisionManager::registerObject(CollisionObject* obj) { current = new_sap->lo; EndPoint* curr_hi = new_sap->hi; - FCL_REAL curr_hi_val = curr_hi->getVal()[coord]; + CoalScalar curr_hi_val = curr_hi->getVal()[coord]; if (coord == 0) { while ((current->getVal()[coord] < curr_hi_val) && @@ -274,7 +273,7 @@ void SaPCollisionManager::registerObject(CollisionObject* obj) { void SaPCollisionManager::setup() { if (size() == 0) return; - FCL_REAL scale[3]; + CoalScalar scale[3]; scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0); scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1); scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2); @@ -293,8 +292,8 @@ void SaPCollisionManager::update_(SaPAABB* updated_aabb) { const AABB current_aabb = current->obj->getAABB(); - const Vec3f& new_min = current_aabb.min_; - const Vec3f& new_max = current_aabb.max_; + const Vec3s& new_min = current_aabb.min_; + const Vec3s& new_max = current_aabb.max_; for (int coord = 0; coord < 3; ++coord) { int direction; // -1 reverse, 0 nochange, 1 forward @@ -507,8 +506,8 @@ bool SaPCollisionManager::collide_(CollisionObject* obj, int axis = optimal_axis; const AABB& obj_aabb = obj->getAABB(); - FCL_REAL min_val = obj_aabb.min_[axis]; - // FCL_REAL max_val = obj_aabb.max_[axis]; + CoalScalar min_val = obj_aabb.min_[axis]; + // CoalScalar max_val = obj_aabb.max_[axis]; EndPoint dummy; SaPAABB dummy_aabb; @@ -520,11 +519,11 @@ bool SaPCollisionManager::collide_(CollisionObject* obj, // iteration linearly const auto res_it = std::upper_bound( velist[axis].begin(), velist[axis].end(), &dummy, - std::bind(std::less(), - std::bind(static_cast( + std::bind(std::less(), + std::bind(static_cast( &EndPoint::getVal), std::placeholders::_1, axis), - std::bind(static_cast( + std::bind(static_cast( &EndPoint::getVal), std::placeholders::_2, axis))); @@ -585,26 +584,26 @@ void SaPCollisionManager::collide(CollisionObject* obj, //============================================================================== bool SaPCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const { - Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; + CoalScalar& min_dist) const { + Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; AABB aabb = obj->getAABB(); - if (min_dist < (std::numeric_limits::max)()) { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); + if (min_dist < (std::numeric_limits::max)()) { + Vec3s min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } int axis = optimal_axis; int status = 1; - FCL_REAL old_min_distance; + CoalScalar old_min_distance; EndPoint* start_pos = elist[axis]; while (1) { old_min_distance = min_dist; - FCL_REAL min_val = aabb.min_[axis]; - // FCL_REAL max_val = aabb.max_[axis]; + CoalScalar min_val = aabb.min_[axis]; + // CoalScalar max_val = aabb.max_[axis]; EndPoint dummy; SaPAABB dummy_aabb; @@ -614,11 +613,11 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, const auto res_it = std::upper_bound( velist[axis].begin(), velist[axis].end(), &dummy, - std::bind(std::less(), - std::bind(static_cast( + std::bind(std::less(), + std::bind(static_cast( &EndPoint::getVal), std::placeholders::_1, axis), - std::bind(static_cast( + std::bind(static_cast( &EndPoint::getVal), std::placeholders::_2, axis))); @@ -653,11 +652,11 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, } if (status == 1) { - if (old_min_distance < (std::numeric_limits::max)()) + if (old_min_distance < (std::numeric_limits::max)()) break; else { if (min_dist < old_min_distance) { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); + Vec3s min_dist_delta(min_dist, min_dist, min_dist); aabb = AABB(obj->getAABB(), min_dist_delta); status = 0; } else { @@ -680,7 +679,7 @@ void SaPCollisionManager::distance(CollisionObject* obj, callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); distance_(obj, callback, min_dist); } @@ -707,7 +706,7 @@ void SaPCollisionManager::distance(DistanceCallBackBase* callback) const { this->enable_tested_set_ = true; this->tested_set.clear(); - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) { if (distance_((*it)->obj, callback, min_dist)) break; @@ -758,7 +757,7 @@ void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, return; } - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); if (this->size() < other_manager->size()) { for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) { @@ -780,7 +779,7 @@ bool SaPCollisionManager::empty() const { return AABB_arr.empty(); } size_t SaPCollisionManager::size() const { return AABB_arr.size(); } //============================================================================== -const Vec3f& SaPCollisionManager::EndPoint::getVal() const { +const Vec3s& SaPCollisionManager::EndPoint::getVal() const { if (minmax) return aabb->cached.max_; else @@ -788,7 +787,7 @@ const Vec3f& SaPCollisionManager::EndPoint::getVal() const { } //============================================================================== -Vec3f& SaPCollisionManager::EndPoint::getVal() { +Vec3s& SaPCollisionManager::EndPoint::getVal() { if (minmax) return aabb->cached.max_; else @@ -796,7 +795,7 @@ Vec3f& SaPCollisionManager::EndPoint::getVal() { } //============================================================================== -FCL_REAL SaPCollisionManager::EndPoint::getVal(int i) const { +CoalScalar SaPCollisionManager::EndPoint::getVal(int i) const { if (minmax) return aabb->cached.max_[i]; else @@ -804,7 +803,7 @@ FCL_REAL SaPCollisionManager::EndPoint::getVal(int i) const { } //============================================================================== -FCL_REAL& SaPCollisionManager::EndPoint::getVal(int i) { +CoalScalar& SaPCollisionManager::EndPoint::getVal(int i) { if (minmax) return aabb->cached.max_[i]; else @@ -849,6 +848,4 @@ bool SaPCollisionManager::isNotValidPair::operator()(const SaPPair& pair) { return (pair.obj1 == obj1) && (pair.obj2 == obj2); } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/broadphase_bruteforce.cpp b/src/broadphase/broadphase_bruteforce.cpp index 17e50dda1..7d83629bf 100644 --- a/src/broadphase/broadphase_bruteforce.cpp +++ b/src/broadphase/broadphase_bruteforce.cpp @@ -35,13 +35,12 @@ /** @author Jia Pan */ -#include "hpp/fcl/broadphase/broadphase_bruteforce.h" +#include "coal/broadphase/broadphase_bruteforce.h" #include #include -namespace hpp { -namespace fcl { +namespace coal { //============================================================================== NaiveCollisionManager::NaiveCollisionManager() { @@ -101,7 +100,7 @@ void NaiveCollisionManager::distance(CollisionObject* obj, callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); for (auto* obj2 : objs) { if (obj->getAABB().distance(obj2->getAABB()) < min_dist) { if ((*callback)(obj, obj2, min_dist)) return; @@ -132,7 +131,7 @@ void NaiveCollisionManager::distance(DistanceCallBackBase* callback) const { callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); for (typename std::list::const_iterator it1 = objs.begin(), end = objs.end(); it1 != end; ++it1) { @@ -183,7 +182,7 @@ void NaiveCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, return; } - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); for (auto* obj1 : objs) { for (auto* obj2 : other_manager->objs) { if (obj1->getAABB().distance(obj2->getAABB()) < min_dist) { @@ -199,5 +198,4 @@ bool NaiveCollisionManager::empty() const { return objs.empty(); } //============================================================================== size_t NaiveCollisionManager::size() const { return objs.size(); } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/broadphase_collision_manager.cpp b/src/broadphase/broadphase_collision_manager.cpp index 90f03249c..afb0a868f 100644 --- a/src/broadphase/broadphase_collision_manager.cpp +++ b/src/broadphase/broadphase_collision_manager.cpp @@ -35,10 +35,9 @@ /** @author Jia Pan */ -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" +#include "coal/broadphase/broadphase_collision_manager.h" -namespace hpp { -namespace fcl { +namespace coal { //============================================================================== BroadPhaseCollisionManager::BroadPhaseCollisionManager() @@ -59,7 +58,7 @@ void BroadPhaseCollisionManager::registerObjects( //============================================================================== void BroadPhaseCollisionManager::update(CollisionObject* updated_obj) { - HPP_FCL_UNUSED_VARIABLE(updated_obj); + COAL_UNUSED_VARIABLE(updated_obj); update(); } @@ -67,7 +66,7 @@ void BroadPhaseCollisionManager::update(CollisionObject* updated_obj) { //============================================================================== void BroadPhaseCollisionManager::update( const std::vector& updated_objs) { - HPP_FCL_UNUSED_VARIABLE(updated_objs); + COAL_UNUSED_VARIABLE(updated_objs); update(); } @@ -90,5 +89,4 @@ void BroadPhaseCollisionManager::insertTestedSet(CollisionObject* a, tested_set.insert(std::make_pair(b, a)); } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index 608d4628f..755d2763f 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -35,28 +35,27 @@ /** @author Jia Pan */ -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree.h" -#include - -#ifdef HPP_FCL_HAVE_OCTOMAP -#include "hpp/fcl/octree.h" +#ifdef COAL_HAVE_OCTOMAP +#include "coal/octree.h" #endif -#include "hpp/fcl/BV/BV.h" -#include "hpp/fcl/shape/geometric_shapes_utility.h" +#include "coal/BV/BV.h" +#include "coal/shape/geometric_shapes_utility.h" + +#include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { namespace dynamic_AABB_tree { -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP //============================================================================== bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Transform3f& tf2, + const AABB& root2_bv, const Transform3s& tf2, CollisionCallBackBase* callback) { if (!root2) { if (root1->isLeaf()) { @@ -64,12 +63,12 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, if (!obj1->collisionGeometry()->isFree()) { OBB obb1, obb2; - convertBV(root1->bv, Transform3f::Identity(), obb1); + convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (obb1.overlap(obb2)) { Box* box = new Box(); - Transform3f box_tf; + Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = tree2->getDefaultOccupancy(); @@ -93,12 +92,12 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { OBB obb1, obb2; - convertBV(root1->bv, Transform3f::Identity(), obb1); + convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (obb1.overlap(obb2)) { Box* box = new Box(); - Transform3f box_tf; + Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = root2->getOccupancy(); @@ -113,7 +112,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, } OBB obb1, obb2; - convertBV(root1->bv, Transform3f::Identity(), obb1); + convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; @@ -149,12 +148,12 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, //============================================================================== bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Transform3f& tf2, - DistanceCallBackBase* callback, FCL_REAL& min_dist) { + const AABB& root2_bv, const Transform3s& tf2, + DistanceCallBackBase* callback, CoalScalar& min_dist) { if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if (tree2->isNodeOccupied(root2)) { Box* box = new Box(); - Transform3f box_tf; + Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(shared_ptr(box), box_tf); return (*callback)(static_cast(root1->data), &obj, @@ -170,8 +169,8 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, AABB aabb2; convertBV(root2_bv, tf2, aabb2); - FCL_REAL d1 = aabb2.distance(root1->children[0]->bv); - FCL_REAL d2 = aabb2.distance(root1->children[1]->bv); + CoalScalar d1 = aabb2.distance(root1->children[0]->bv); + CoalScalar d2 = aabb2.distance(root1->children[1]->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -207,7 +206,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, AABB aabb2; convertBV(child_bv, tf2, aabb2); - FCL_REAL d = root1->bv.distance(aabb2); + CoalScalar d = root1->bv.distance(aabb2); if (d < min_dist) { if (distanceRecurse_(root1, tree2, child, child_bv, tf2, callback, @@ -224,7 +223,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, //============================================================================== bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Transform3f& tf2, + const AABB& root2_bv, const Transform3s& tf2, CollisionCallBackBase* callback) { if (tf2.rotation().isIdentity()) return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.translation(), @@ -236,8 +235,8 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, //============================================================================== bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Transform3f& tf2, - DistanceCallBackBase* callback, FCL_REAL& min_dist) { + const AABB& root2_bv, const Transform3s& tf2, + DistanceCallBackBase* callback, CoalScalar& min_dist) { if (tf2.rotation().isIdentity()) return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.translation(), callback, min_dist); @@ -408,7 +407,7 @@ bool selfCollisionRecurse( //============================================================================== bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, - DistanceCallBackBase* callback, FCL_REAL& min_dist) { + DistanceCallBackBase* callback, CoalScalar& min_dist) { if (root1->isLeaf() && root2->isLeaf()) { CollisionObject* root1_obj = static_cast(root1->data); CollisionObject* root2_obj = static_cast(root2->data); @@ -417,8 +416,8 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, if (root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { - FCL_REAL d1 = root2->bv.distance(root1->children[0]->bv); - FCL_REAL d2 = root2->bv.distance(root1->children[1]->bv); + CoalScalar d1 = root2->bv.distance(root1->children[0]->bv); + CoalScalar d2 = root2->bv.distance(root1->children[1]->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -442,8 +441,8 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, } } } else { - FCL_REAL d1 = root1->bv.distance(root2->children[0]->bv); - FCL_REAL d2 = root1->bv.distance(root2->children[1]->bv); + CoalScalar d1 = root1->bv.distance(root2->children[0]->bv); + CoalScalar d2 = root1->bv.distance(root2->children[1]->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -474,14 +473,14 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, //============================================================================== bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, DistanceCallBackBase* callback, - FCL_REAL& min_dist) { + CoalScalar& min_dist) { if (root->isLeaf()) { CollisionObject* root_obj = static_cast(root->data); return (*callback)(root_obj, query, min_dist); } - FCL_REAL d1 = query->getAABB().distance(root->children[0]->bv); - FCL_REAL d2 = query->getAABB().distance(root->children[1]->bv); + CoalScalar d1 = query->getAABB().distance(root->children[0]->bv); + CoalScalar d2 = query->getAABB().distance(root->children[1]->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -510,7 +509,7 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, //============================================================================== bool selfDistanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, - DistanceCallBackBase* callback, FCL_REAL& min_dist) { + DistanceCallBackBase* callback, CoalScalar& min_dist) { if (root->isLeaf()) return false; if (selfDistanceRecurse(root->children[0], callback, min_dist)) return true; @@ -594,7 +593,7 @@ void DynamicAABBTreeCollisionManager::setup() { size_t height = dtree.getMaxHeight(); - if (((FCL_REAL)height - std::log((FCL_REAL)num) / std::log(2.0)) < + if (((CoalScalar)height - std::log((CoalScalar)num) / std::log(2.0)) < max_tree_nonbalanced_level) dtree.balanceIncremental(tree_incremental_balance_pass); else @@ -611,8 +610,8 @@ void DynamicAABBTreeCollisionManager::update() { DynamicAABBNode* node = it->second; node->bv = obj->getAABB(); if (node->bv.volume() <= 0.) - HPP_FCL_THROW_PRETTY("The bounding volume has a negative volume.", - std::invalid_argument) + COAL_THROW_PRETTY("The bounding volume has a negative volume.", + std::invalid_argument) } dtree.refit(); @@ -667,7 +666,7 @@ void DynamicAABBTreeCollisionManager::collide( callback->init(); if (size() == 0) return; switch (obj->collisionGeometry()->getNodeType()) { -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP case GEOM_OCTREE: { if (!octree_as_geometry_collide) { const OcTree* octree = @@ -691,9 +690,9 @@ void DynamicAABBTreeCollisionManager::distance( CollisionObject* obj, DistanceCallBackBase* callback) const { callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); switch (obj->collisionGeometry()->getNodeType()) { -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP case GEOM_OCTREE: { if (!octree_as_geometry_distance) { const OcTree* octree = @@ -725,7 +724,7 @@ void DynamicAABBTreeCollisionManager::distance( DistanceCallBackBase* callback) const { callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); detail::dynamic_AABB_tree::selfDistanceRecurse(dtree.getRoot(), callback, min_dist); } @@ -750,7 +749,7 @@ void DynamicAABBTreeCollisionManager::distance( DynamicAABBTreeCollisionManager* other_manager = static_cast(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); detail::dynamic_AABB_tree::distanceRecurse( dtree.getRoot(), other_manager->dtree.getRoot(), callback, min_dist); } @@ -771,5 +770,4 @@ detail::HierarchyTree& DynamicAABBTreeCollisionManager::getTree() { return dtree; } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp index 492140837..9306fa8eb 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp @@ -35,23 +35,22 @@ /** @author Jia Pan */ -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" -#ifdef HPP_FCL_HAVE_OCTOMAP -#include "hpp/fcl/octree.h" +#ifdef COAL_HAVE_OCTOMAP +#include "coal/octree.h" #endif -namespace hpp { -namespace fcl { +namespace coal { namespace detail { namespace dynamic_AABB_tree_array { -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP //============================================================================== bool collisionRecurse_( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Transform3f& tf2, + const AABB& root2_bv, const Transform3s& tf2, CollisionCallBackBase* callback) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; @@ -61,12 +60,12 @@ bool collisionRecurse_( if (!obj1->collisionGeometry()->isFree()) { OBB obb1, obb2; - convertBV(root1->bv, Transform3f::Identity(), obb1); + convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (obb1.overlap(obb2)) { Box* box = new Box(); - Transform3f box_tf; + Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = tree2->getDefaultOccupancy(); @@ -89,12 +88,12 @@ bool collisionRecurse_( CollisionObject* obj1 = static_cast(root1->data); if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { OBB obb1, obb2; - convertBV(root1->bv, Transform3f::Identity(), obb1); + convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (obb1.overlap(obb2)) { Box* box = new Box(); - Transform3f box_tf; + Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = root2->getOccupancy(); @@ -109,7 +108,7 @@ bool collisionRecurse_( } OBB obb1, obb2; - convertBV(root1->bv, Transform3f::Identity(), obb1); + convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; @@ -149,14 +148,14 @@ bool collisionRecurse_( bool distanceRecurse_( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Transform3f& tf2, - DistanceCallBackBase* callback, FCL_REAL& min_dist) { + const AABB& root2_bv, const Transform3s& tf2, + DistanceCallBackBase* callback, CoalScalar& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if (tree2->isNodeOccupied(root2)) { Box* box = new Box(); - Transform3f box_tf; + Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(shared_ptr(box), box_tf); return (*callback)(static_cast(root1->data), &obj, @@ -172,8 +171,8 @@ bool distanceRecurse_( AABB aabb2; convertBV(root2_bv, tf2, aabb2); - FCL_REAL d1 = aabb2.distance((nodes1 + root1->children[0])->bv); - FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv); + CoalScalar d1 = aabb2.distance((nodes1 + root1->children[0])->bv); + CoalScalar d2 = aabb2.distance((nodes1 + root1->children[1])->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -209,7 +208,7 @@ bool distanceRecurse_( AABB aabb2; convertBV(child_bv, tf2, aabb2); - FCL_REAL d = root1->bv.distance(aabb2); + CoalScalar d = root1->bv.distance(aabb2); if (d < min_dist) { if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, @@ -263,7 +262,7 @@ bool collisionRecurse( } //============================================================================== -inline HPP_FCL_DLLAPI bool collisionRecurse( +inline COAL_DLLAPI bool collisionRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, CollisionCallBackBase* callback) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; @@ -308,7 +307,7 @@ bool distanceRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes2, - size_t root2_id, DistanceCallBackBase* callback, FCL_REAL& min_dist) { + size_t root2_id, DistanceCallBackBase* callback, CoalScalar& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root2 = @@ -321,8 +320,8 @@ bool distanceRecurse( if (root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { - FCL_REAL d1 = root2->bv.distance((nodes1 + root1->children[0])->bv); - FCL_REAL d2 = root2->bv.distance((nodes1 + root1->children[1])->bv); + CoalScalar d1 = root2->bv.distance((nodes1 + root1->children[0])->bv); + CoalScalar d2 = root2->bv.distance((nodes1 + root1->children[1])->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -350,8 +349,8 @@ bool distanceRecurse( } } } else { - FCL_REAL d1 = root1->bv.distance((nodes2 + root2->children[0])->bv); - FCL_REAL d2 = root1->bv.distance((nodes2 + root2->children[1])->bv); + CoalScalar d1 = root1->bv.distance((nodes2 + root2->children[0])->bv); + CoalScalar d2 = root1->bv.distance((nodes2 + root2->children[1])->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -387,15 +386,15 @@ bool distanceRecurse( bool distanceRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, DistanceCallBackBase* callback, - FCL_REAL& min_dist) { + CoalScalar& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; if (root->isLeaf()) { CollisionObject* root_obj = static_cast(root->data); return (*callback)(root_obj, query, min_dist); } - FCL_REAL d1 = query->getAABB().distance((nodes + root->children[0])->bv); - FCL_REAL d2 = query->getAABB().distance((nodes + root->children[1])->bv); + CoalScalar d1 = query->getAABB().distance((nodes + root->children[0])->bv); + CoalScalar d2 = query->getAABB().distance((nodes + root->children[1])->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -425,7 +424,7 @@ bool distanceRecurse( //============================================================================== bool selfDistanceRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, - size_t root_id, DistanceCallBackBase* callback, FCL_REAL& min_dist) { + size_t root_id, DistanceCallBackBase* callback, CoalScalar& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; if (root->isLeaf()) return false; @@ -442,13 +441,13 @@ bool selfDistanceRecurse( return false; } -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP //============================================================================== bool collisionRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Transform3f& tf2, + const AABB& root2_bv, const Transform3s& tf2, CollisionCallBackBase* callback) { if (tf2.rotation().isIdentity()) return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, @@ -462,8 +461,8 @@ bool collisionRecurse( bool distanceRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Transform3f& tf2, - DistanceCallBackBase* callback, FCL_REAL& min_dist) { + const AABB& root2_bv, const Transform3s& tf2, + DistanceCallBackBase* callback, CoalScalar& min_dist) { if (tf2.rotation().isIdentity()) return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.translation(), callback, min_dist); @@ -546,7 +545,7 @@ void DynamicAABBTreeArrayCollisionManager::setup() { int height = (int)dtree.getMaxHeight(); - if ((FCL_REAL)height - std::log((FCL_REAL)num) / std::log(2.0) < + if ((CoalScalar)height - std::log((CoalScalar)num) / std::log(2.0) < max_tree_nonbalanced_level) dtree.balanceIncremental(tree_incremental_balance_pass); else @@ -618,7 +617,7 @@ void DynamicAABBTreeArrayCollisionManager::collide( callback->init(); if (size() == 0) return; switch (obj->collisionGeometry()->getNodeType()) { -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP case GEOM_OCTREE: { if (!octree_as_geometry_collide) { const OcTree* octree = @@ -642,9 +641,9 @@ void DynamicAABBTreeArrayCollisionManager::distance( CollisionObject* obj, DistanceCallBackBase* callback) const { callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); switch (obj->collisionGeometry()->getNodeType()) { -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP case GEOM_OCTREE: { if (!octree_as_geometry_distance) { const OcTree* octree = @@ -677,7 +676,7 @@ void DynamicAABBTreeArrayCollisionManager::distance( DistanceCallBackBase* callback) const { callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); detail::dynamic_AABB_tree_array::selfDistanceRecurse( dtree.getNodes(), dtree.getRoot(), callback, min_dist); } @@ -703,7 +702,7 @@ void DynamicAABBTreeArrayCollisionManager::distance( DynamicAABBTreeArrayCollisionManager* other_manager = static_cast(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); detail::dynamic_AABB_tree_array::distanceRecurse( dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), callback, min_dist); @@ -725,5 +724,4 @@ DynamicAABBTreeArrayCollisionManager::getTree() const { return dtree; } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp index aff1c6d25..5f65e1be9 100644 --- a/src/broadphase/broadphase_interval_tree.cpp +++ b/src/broadphase/broadphase_interval_tree.cpp @@ -35,10 +35,9 @@ /** @author Jia Pan */ -#include "hpp/fcl/broadphase/broadphase_interval_tree.h" +#include "coal/broadphase/broadphase_interval_tree.h" -namespace hpp { -namespace fcl { +namespace coal { //============================================================================== void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) { @@ -363,25 +362,25 @@ void IntervalTreeCollisionManager::distance( CollisionObject* obj, DistanceCallBackBase* callback) const { callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); distance_(obj, callback, min_dist); } //============================================================================== bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const { + CoalScalar& min_dist) const { static const unsigned int CUTOFF = 100; - Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; + Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; AABB aabb = obj->getAABB(); - if (min_dist < (std::numeric_limits::max)()) { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); + if (min_dist < (std::numeric_limits::max)()) { + Vec3s min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } int status = 1; - FCL_REAL old_min_distance; + CoalScalar old_min_distance; while (1) { bool dist_res = false; @@ -426,11 +425,11 @@ bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, results2.clear(); if (status == 1) { - if (old_min_distance < (std::numeric_limits::max)()) + if (old_min_distance < (std::numeric_limits::max)()) break; else { if (min_dist < old_min_distance) { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); + Vec3s min_dist_delta(min_dist, min_dist, min_dist); aabb = AABB(obj->getAABB(), min_dist_delta); status = 0; } else { @@ -456,9 +455,9 @@ void IntervalTreeCollisionManager::collide( std::set active; std::set > overlap; size_t n = endpoints[0].size(); - FCL_REAL diff_x = endpoints[0][0].value - endpoints[0][n - 1].value; - FCL_REAL diff_y = endpoints[1][0].value - endpoints[1][n - 1].value; - FCL_REAL diff_z = endpoints[2][0].value - endpoints[2][n - 1].value; + CoalScalar diff_x = endpoints[0][0].value - endpoints[0][n - 1].value; + CoalScalar diff_y = endpoints[1][0].value - endpoints[1][n - 1].value; + CoalScalar diff_z = endpoints[2][0].value - endpoints[2][n - 1].value; int axis = 0; if (diff_y > diff_x && diff_y > diff_z) @@ -509,7 +508,7 @@ void IntervalTreeCollisionManager::distance( this->enable_tested_set_ = true; this->tested_set.clear(); - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); for (size_t i = 0; i < endpoints[0].size(); ++i) if (distance_(endpoints[0][i].obj, callback, min_dist)) break; @@ -557,7 +556,7 @@ void IntervalTreeCollisionManager::distance( return; } - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); if (this->size() < other_manager->size()) { for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) @@ -604,7 +603,7 @@ bool IntervalTreeCollisionManager::checkDist( typename std::deque::const_iterator pos_start, typename std::deque::const_iterator pos_end, CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const { + CoalScalar& min_dist) const { while (pos_start < pos_end) { SAPInterval* ivl = static_cast(*pos_start); if (ivl->obj != obj) { @@ -636,8 +635,8 @@ bool IntervalTreeCollisionManager::EndPoint::operator<( } //============================================================================== -IntervalTreeCollisionManager::SAPInterval::SAPInterval(FCL_REAL low_, - FCL_REAL high_, +IntervalTreeCollisionManager::SAPInterval::SAPInterval(CoalScalar low_, + CoalScalar high_, CollisionObject* obj_) : detail::SimpleInterval() { this->low = low_; @@ -645,5 +644,4 @@ IntervalTreeCollisionManager::SAPInterval::SAPInterval(FCL_REAL low_, obj = obj_; } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/default_broadphase_callbacks.cpp b/src/broadphase/default_broadphase_callbacks.cpp index ea88687d4..8e54a47e2 100644 --- a/src/broadphase/default_broadphase_callbacks.cpp +++ b/src/broadphase/default_broadphase_callbacks.cpp @@ -34,11 +34,10 @@ /** @author Sean Curtis (sean@tri.global) */ -#include "hpp/fcl/broadphase/default_broadphase_callbacks.h" +#include "coal/broadphase/default_broadphase_callbacks.h" #include -namespace hpp { -namespace fcl { +namespace coal { bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* data) { @@ -65,7 +64,7 @@ bool CollisionCallBackDefault::collide(CollisionObject* o1, } bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, - void* data, FCL_REAL& dist) { + void* data, CoalScalar& dist) { assert(data != nullptr); auto* cdata = static_cast(data); const DistanceRequest& request = cdata->request; @@ -86,7 +85,7 @@ bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, } bool DistanceCallBackDefault::distance(CollisionObject* o1, CollisionObject* o2, - FCL_REAL& dist) { + CoalScalar& dist) { return defaultDistanceFunction(o1, o2, &data, dist); } @@ -122,5 +121,4 @@ bool CollisionCallBackCollect::exist(const CollisionPair& pair) const { collision_pairs.end(); } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/detail/interval_tree.cpp b/src/broadphase/detail/interval_tree.cpp index 567195988..34a2326c4 100644 --- a/src/broadphase/detail/interval_tree.cpp +++ b/src/broadphase/detail/interval_tree.cpp @@ -35,15 +35,14 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_INTERVAL_TREE_INL_H -#define HPP_FCL_INTERVAL_TREE_INL_H +#ifndef COAL_INTERVAL_TREE_INL_H +#define COAL_INTERVAL_TREE_INL_H -#include "hpp/fcl/broadphase/detail/interval_tree.h" +#include "coal/broadphase/detail/interval_tree.h" #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { //============================================================================== @@ -53,13 +52,13 @@ IntervalTree::IntervalTree() { invalid_node; invalid_node->red = false; invalid_node->key = invalid_node->high = invalid_node->max_high = - -(std::numeric_limits::max)(); + -(std::numeric_limits::max)(); invalid_node->stored_interval = nullptr; root = new IntervalTreeNode; root->parent = root->left = root->right = invalid_node; root->key = root->high = root->max_high = - (std::numeric_limits::max)(); + (std::numeric_limits::max)(); root->red = false; root->stored_interval = nullptr; @@ -382,7 +381,7 @@ SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z) { /// @brief y should not be invalid_node in this case /// y is the node to splice out and x is its child if (y != z) { - y->max_high = -(std::numeric_limits::max)(); + y->max_high = -(std::numeric_limits::max)(); y->left = z->left; y->right = z->right; y->parent = z->parent; @@ -410,7 +409,7 @@ SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z) { //============================================================================== /// @brief returns 1 if the intervals overlap, and 0 otherwise -bool overlap(FCL_REAL a1, FCL_REAL a2, FCL_REAL b1, FCL_REAL b2) { +bool overlap(CoalScalar a1, CoalScalar a2, CoalScalar b1, CoalScalar b2) { if (a1 <= b1) { return (b1 <= a2); } else { @@ -419,7 +418,8 @@ bool overlap(FCL_REAL a1, FCL_REAL a2, FCL_REAL b1, FCL_REAL b2) { } //============================================================================== -std::deque IntervalTree::query(FCL_REAL low, FCL_REAL high) { +std::deque IntervalTree::query(CoalScalar low, + CoalScalar high) { std::deque result_stack; IntervalTreeNode* x = root->left; bool run = (x != invalid_node); @@ -463,7 +463,6 @@ std::deque IntervalTree::query(FCL_REAL low, FCL_REAL high) { } } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/src/broadphase/detail/interval_tree_node.cpp b/src/broadphase/detail/interval_tree_node.cpp index beb9e28d6..51a1a457a 100644 --- a/src/broadphase/detail/interval_tree_node.cpp +++ b/src/broadphase/detail/interval_tree_node.cpp @@ -35,16 +35,15 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H -#define HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H +#ifndef COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H +#define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H -#include "hpp/fcl/broadphase/detail/interval_tree_node.h" +#include "coal/broadphase/detail/interval_tree_node.h" #include #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { //============================================================================== @@ -90,7 +89,6 @@ void IntervalTreeNode::print(IntervalTreeNode* invalid_node, } } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/src/broadphase/detail/morton-inl.h b/src/broadphase/detail/morton-inl.h index 53d7d90f7..b646f299e 100644 --- a/src/broadphase/detail/morton-inl.h +++ b/src/broadphase/detail/morton-inl.h @@ -36,13 +36,12 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_MORTON_INL_H -#define HPP_FCL_MORTON_INL_H +#ifndef COAL_MORTON_INL_H +#define COAL_MORTON_INL_H -#include "hpp/fcl/broadphase/detail/morton.h" +#include "coal/broadphase/detail/morton.h" -namespace hpp { -namespace fcl { /// @cond IGNORE +namespace coal { /// @cond IGNORE namespace detail { //============================================================================== @@ -63,7 +62,7 @@ morton_functor::morton_functor(const AABB& bbox) //============================================================================== template -uint32_t morton_functor::operator()(const Vec3f& point) const { +uint32_t morton_functor::operator()(const Vec3s& point) const { uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u); uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u); uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u); @@ -83,7 +82,7 @@ morton_functor::morton_functor(const AABB& bbox) //============================================================================== template -uint64_t morton_functor::operator()(const Vec3f& point) const { +uint64_t morton_functor::operator()(const Vec3s& point) const { uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20); uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20); uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20); @@ -116,7 +115,7 @@ morton_functor>::morton_functor(const AABB& bbox) //============================================================================== template std::bitset morton_functor>::operator()( - const Vec3f& point) const { + const Vec3s& point) const { S x = (point[0] - base[0]) * inv[0]; S y = (point[1] - base[1]) * inv[1]; S z = (point[2] - base[2]) * inv[2]; @@ -147,7 +146,6 @@ constexpr size_t morton_functor>::bits() { } // namespace detail /// @endcond -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/src/broadphase/detail/morton.cpp b/src/broadphase/detail/morton.cpp index 54beaa104..17e554f0e 100644 --- a/src/broadphase/detail/morton.cpp +++ b/src/broadphase/detail/morton.cpp @@ -36,10 +36,9 @@ /** @author Jia Pan */ -#include "hpp/fcl/broadphase/detail/morton-inl.h" +#include "coal/broadphase/detail/morton-inl.h" -namespace hpp { -namespace fcl { +namespace coal { /// @cond IGNORE namespace detail { @@ -79,5 +78,4 @@ uint64_t morton_code60(uint32_t x, uint32_t y, uint32_t z) { } // namespace detail /// @endcond -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/detail/simple_interval.cpp b/src/broadphase/detail/simple_interval.cpp index 78719156c..95075cbc1 100644 --- a/src/broadphase/detail/simple_interval.cpp +++ b/src/broadphase/detail/simple_interval.cpp @@ -35,14 +35,13 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H -#define HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H +#ifndef COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H +#define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H -#include "hpp/fcl/broadphase/detail/simple_interval.h" +#include "coal/broadphase/detail/simple_interval.h" #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { //============================================================================== @@ -56,7 +55,6 @@ void SimpleInterval::print() { } } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/src/broadphase/detail/spatial_hash.cpp b/src/broadphase/detail/spatial_hash.cpp index 07f7b6631..dd92fb7c5 100644 --- a/src/broadphase/detail/spatial_hash.cpp +++ b/src/broadphase/detail/spatial_hash.cpp @@ -35,18 +35,17 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_SPATIALHASH_INL_H -#define HPP_FCL_BROADPHASE_SPATIALHASH_INL_H +#ifndef COAL_BROADPHASE_SPATIALHASH_INL_H +#define COAL_BROADPHASE_SPATIALHASH_INL_H -#include "hpp/fcl/broadphase/detail/spatial_hash.h" +#include "coal/broadphase/detail/spatial_hash.h" #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { //============================================================================== -SpatialHash::SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_) +SpatialHash::SpatialHash(const AABB& scene_limit_, CoalScalar cell_size_) : cell_size(cell_size_), scene_limit(scene_limit_) { width[0] = static_cast(std::ceil(scene_limit.width() / cell_size)); @@ -85,7 +84,6 @@ std::vector SpatialHash::operator()(const AABB& aabb) const { } } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/src/collision.cpp b/src/collision.cpp index 9db29dd7b..95085f3be 100644 --- a/src/collision.cpp +++ b/src/collision.cpp @@ -35,13 +35,12 @@ /** \author Jia Pan */ -#include -#include -#include -#include +#include "coal/collision.h" +#include "coal/collision_utility.h" +#include "coal/collision_func_matrix.h" +#include "coal/narrowphase/narrowphase.h" -namespace hpp { -namespace fcl { +namespace coal { CollisionFunctionMatrix& getCollisionFunctionLookTable() { static CollisionFunctionMatrix table; @@ -66,11 +65,11 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, result); } -std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +std::size_t collide(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { // If security margin is set to -infinity, return that there is no collision - if (request.security_margin == -std::numeric_limits::infinity()) { + if (request.security_margin == -std::numeric_limits::infinity()) { result.clear(); return false; } @@ -80,8 +79,8 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable(); std::size_t res; if (request.num_max_contacts == 0) { - HPP_FCL_THROW_PRETTY("Invalid number of max contacts (current value is 0).", - std::invalid_argument); + COAL_THROW_PRETTY("Invalid number of max contacts (current value is 0).", + std::invalid_argument); res = 0; } else { OBJECT_TYPE object_type1 = o1->getObjectType(); @@ -92,12 +91,12 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, if (object_type1 == OT_GEOM && (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) { if (!looktable.collision_matrix[node_type2][node_type1]) { - HPP_FCL_THROW_PRETTY("Collision function between node type " - << std::string(get_node_type_name(node_type1)) - << " and node type " - << std::string(get_node_type_name(node_type2)) - << " is not yet supported.", - std::invalid_argument); + COAL_THROW_PRETTY("Collision function between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); res = 0; } else { res = looktable.collision_matrix[node_type2][node_type1]( @@ -108,12 +107,12 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, } } else { if (!looktable.collision_matrix[node_type1][node_type2]) { - HPP_FCL_THROW_PRETTY("Collision function between node type " - << std::string(get_node_type_name(node_type1)) - << " and node type " - << std::string(get_node_type_name(node_type2)) - << " is not yet supported.", - std::invalid_argument); + COAL_THROW_PRETTY("Collision function between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); res = 0; } else res = looktable.collision_matrix[node_type1][node_type2]( @@ -144,12 +143,12 @@ ComputeCollision::ComputeCollision(const CollisionGeometry* o1, if ((swap_geoms && !looktable.collision_matrix[node_type2][node_type1]) || (!swap_geoms && !looktable.collision_matrix[node_type1][node_type2])) { - HPP_FCL_THROW_PRETTY("Collision function between node type " - << std::string(get_node_type_name(node_type1)) - << " and node type " - << std::string(get_node_type_name(node_type2)) - << " is not yet supported.", - std::invalid_argument); + COAL_THROW_PRETTY("Collision function between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); } if (swap_geoms) func = looktable.collision_matrix[node_type2][node_type1]; @@ -157,12 +156,12 @@ ComputeCollision::ComputeCollision(const CollisionGeometry* o1, func = looktable.collision_matrix[node_type1][node_type2]; } -std::size_t ComputeCollision::run(const Transform3f& tf1, - const Transform3f& tf2, +std::size_t ComputeCollision::run(const Transform3s& tf1, + const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) const { // If security margin is set to -infinity, return that there is no collision - if (request.security_margin == -std::numeric_limits::infinity()) { + if (request.security_margin == -std::numeric_limits::infinity()) { result.clear(); return false; } @@ -184,8 +183,8 @@ std::size_t ComputeCollision::run(const Transform3f& tf1, return res; } -std::size_t ComputeCollision::operator()(const Transform3f& tf1, - const Transform3f& tf2, +std::size_t ComputeCollision::operator()(const Transform3s& tf1, + const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) const @@ -203,5 +202,4 @@ std::size_t ComputeCollision::operator()(const Transform3f& tf1, return res; } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/collision_data.cpp b/src/collision_data.cpp index 2572014df..7561ba831 100644 --- a/src/collision_data.cpp +++ b/src/collision_data.cpp @@ -36,10 +36,9 @@ /** \author Jia Pan */ -#include +#include "coal/collision_data.h" -namespace hpp { -namespace fcl { +namespace coal { bool CollisionRequest::isSatisfied(const CollisionResult& result) const { return result.isCollision() && (num_max_contacts <= result.numContacts()); @@ -49,5 +48,4 @@ bool DistanceRequest::isSatisfied(const DistanceResult& result) const { return (result.min_distance <= 0); } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index d9fd859e1..b654cfbda 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -35,32 +35,30 @@ /** \author Jia Pan */ -#include +#include "coal/collision_func_matrix.h" -#include +#include "coal/internal/traversal_node_setup.h" #include <../src/collision_node.h> -#include -#include -#include +#include "coal/narrowphase/narrowphase.h" +#include "coal/internal/shape_shape_func.h" +#include "coal/shape/geometric_shapes_traits.h" #include <../src/traits_traversal.h> -namespace hpp { -namespace fcl { +namespace coal { -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP template -std::size_t OctreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +std::size_t OctreeCollide(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); if (request.security_margin < 0) - HPP_FCL_THROW_PRETTY( - "Negative security margin are not handled yet for Octree", - std::invalid_argument); + COAL_THROW_PRETTY("Negative security margin are not handled yet for Octree", + std::invalid_argument); typename TraversalTraitsCollision::CollisionTraversal_t node( request); @@ -99,26 +97,25 @@ BVH_SHAPE_DEFAULT_TO_ORIENTED(OBBRSS); /// - 0 if the query should be made with non-aligned object frames. template ::Options> -struct HPP_FCL_LOCAL BVHShapeCollider{static std::size_t collide( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +struct COAL_LOCAL BVHShapeCollider{static std::size_t collide( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result){ if (request.isSatisfied(result)) return result.numContacts(); if (request.security_margin < 0) - HPP_FCL_THROW_PRETTY( - "Negative security margin are not handled yet for BVHModel", - std::invalid_argument); + COAL_THROW_PRETTY("Negative security margin are not handled yet for BVHModel", + std::invalid_argument); if (_Options & RelativeTransformationIsIdentity) return aligned(o1, tf1, o2, tf2, nsolver, request, result); else return oriented(o1, tf1, o2, tf2, nsolver, request, result); -} // namespace fcl +} // namespace coal -static std::size_t aligned(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +static std::size_t aligned(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { @@ -128,18 +125,18 @@ static std::size_t aligned(const CollisionGeometry* o1, const Transform3f& tf1, node(request); const BVHModel* obj1 = static_cast*>(o1); BVHModel* obj1_tmp = new BVHModel(*obj1); - Transform3f tf1_tmp = tf1; + Transform3s tf1_tmp = tf1; const T_SH* obj2 = static_cast(o2); initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, result); - fcl::collide(&node, request, result); + coal::collide(&node, request, result); delete obj1_tmp; return result.numContacts(); } -static std::size_t oriented(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +static std::size_t oriented(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { @@ -150,10 +147,11 @@ static std::size_t oriented(const CollisionGeometry* o1, const Transform3f& tf1, const T_SH* obj2 = static_cast(o2); initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result); - fcl::collide(&node, request, result); + coal::collide(&node, request, result); return result.numContacts(); } -}; // namespace hpp +} +; /// @brief Collider functor for HeightField data structure /// \tparam _Options takes two values. @@ -161,13 +159,13 @@ static std::size_t oriented(const CollisionGeometry* o1, const Transform3f& tf1, /// into the frame of object 2 before computing collisions. /// - 0 if the query should be made with non-aligned object frames. template -struct HPP_FCL_LOCAL HeightFieldShapeCollider { +struct COAL_LOCAL HeightFieldShapeCollider { typedef HeightField HF; static std::size_t collide(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, + const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); @@ -178,7 +176,7 @@ struct HPP_FCL_LOCAL HeightFieldShapeCollider { HeightFieldShapeCollisionTraversalNode node(request); initialize(node, height_field, tf1, shape, tf2, nsolver, result); - fcl::collide(&node, request, result); + coal::collide(&node, request, result); return result.numContacts(); } }; @@ -186,9 +184,9 @@ struct HPP_FCL_LOCAL HeightFieldShapeCollider { namespace details { template std::size_t orientedMeshCollide(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); @@ -206,8 +204,8 @@ std::size_t orientedMeshCollide(const CollisionGeometry* o1, } // namespace details template -std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); @@ -223,12 +221,12 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const BVHModel* obj1 = static_cast*>(o1); const BVHModel* obj2 = static_cast*>(o2); BVHModel* obj1_tmp = new BVHModel(*obj1); - Transform3f tf1_tmp = tf1; + Transform3s tf1_tmp = tf1; BVHModel* obj2_tmp = new BVHModel(*obj2); - Transform3f tf2_tmp = tf2; + Transform3s tf2_tmp = tf2; initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, result); - fcl::collide(&node, request, result); + coal::collide(&node, request, result); delete obj1_tmp; delete obj2_tmp; @@ -237,8 +235,8 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, } template <> -std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { return details::orientedMeshCollide( @@ -247,9 +245,9 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, template <> std::size_t BVHCollide(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { return details::orientedMeshCollide( @@ -258,9 +256,9 @@ std::size_t BVHCollide(const CollisionGeometry* o1, template <> std::size_t BVHCollide(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { return details::orientedMeshCollide( @@ -268,8 +266,8 @@ std::size_t BVHCollide(const CollisionGeometry* o1, } template -std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* /*nsolver*/, const CollisionRequest& request, CollisionResult& result) { @@ -657,7 +655,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide; collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide; -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OctreeCollide; @@ -728,6 +726,4 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { #endif } // template struct CollisionFunctionMatrix; -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/collision_node.cpp b/src/collision_node.cpp index 4a016916c..77c6adb04 100644 --- a/src/collision_node.cpp +++ b/src/collision_node.cpp @@ -36,28 +36,26 @@ /** \author Jia Pan */ #include <../src/collision_node.h> -#include +#include "coal/internal/traversal_recurse.h" -namespace hpp { -namespace fcl { +namespace coal { void checkResultLowerBound(const CollisionResult& result, - FCL_REAL sqrDistLowerBound) { - HPP_FCL_UNUSED_VARIABLE(result); - const FCL_REAL dummy_precision = - std::sqrt(Eigen::NumTraits::epsilon()); - HPP_FCL_UNUSED_VARIABLE(dummy_precision); + CoalScalar sqrDistLowerBound) { + COAL_UNUSED_VARIABLE(result); + const CoalScalar dummy_precision = + std::sqrt(Eigen::NumTraits::epsilon()); + COAL_UNUSED_VARIABLE(dummy_precision); if (sqrDistLowerBound == 0) { - HPP_FCL_ASSERT(result.distance_lower_bound <= dummy_precision, - "Distance lower bound should not be positive.", - std::logic_error); + COAL_ASSERT(result.distance_lower_bound <= dummy_precision, + "Distance lower bound should not be positive.", + std::logic_error); } else { - HPP_FCL_ASSERT( - result.distance_lower_bound * result.distance_lower_bound - - sqrDistLowerBound < - dummy_precision, - "Distance lower bound and sqrDistLowerBound should coincide.", - std::logic_error); + COAL_ASSERT(result.distance_lower_bound * result.distance_lower_bound - + sqrDistLowerBound < + dummy_precision, + "Distance lower bound and sqrDistLowerBound should coincide.", + std::logic_error); } } @@ -67,7 +65,7 @@ void collide(CollisionTraversalNodeBase* node, const CollisionRequest& request, if (front_list && front_list->size() > 0) { propagateBVHFrontListCollisionRecurse(node, request, result, front_list); } else { - FCL_REAL sqrDistLowerBound = 0; + CoalScalar sqrDistLowerBound = 0; if (recursive) collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound); else @@ -90,6 +88,4 @@ void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, node->postprocess(); } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/collision_node.h b/src/collision_node.h index 8a72be9cf..b6be94f5d 100644 --- a/src/collision_node.h +++ b/src/collision_node.h @@ -35,20 +35,19 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_COLLISION_NODE_H -#define HPP_FCL_COLLISION_NODE_H +#ifndef COAL_COLLISION_NODE_H +#define COAL_COLLISION_NODE_H /// @cond INTERNAL -#include -#include -#include +#include "coal/BVH/BVH_front.h" +#include "coal/internal/traversal_node_base.h" +#include "coal/internal/traversal_node_bvhs.h" /// @brief collision and distance function on traversal nodes. these functions /// provide a higher level abstraction for collision functions provided in /// collision_func_matrix -namespace hpp { -namespace fcl { +namespace coal { /// collision on collision traversal node /// @@ -57,21 +56,19 @@ namespace fcl { /// do not collide. /// @param front_list list of nodes visited by the query, can be used to /// accelerate computation -/// \todo should be HPP_FCL_LOCAL but used in unit test. -HPP_FCL_DLLAPI void collide(CollisionTraversalNodeBase* node, - const CollisionRequest& request, - CollisionResult& result, - BVHFrontList* front_list = NULL, - bool recursive = true); +/// \todo should be COAL_LOCAL but used in unit test. +COAL_DLLAPI void collide(CollisionTraversalNodeBase* node, + const CollisionRequest& request, + CollisionResult& result, + BVHFrontList* front_list = NULL, + bool recursive = true); /// @brief distance computation on distance traversal node; can use front list -/// to accelerate \todo should be HPP_FCL_LOCAL but used in unit test. -HPP_FCL_DLLAPI void distance(DistanceTraversalNodeBase* node, - BVHFrontList* front_list = NULL, - unsigned int qsize = 2); -} // namespace fcl - -} // namespace hpp +/// to accelerate \todo should be COAL_LOCAL but used in unit test. +COAL_DLLAPI void distance(DistanceTraversalNodeBase* node, + BVHFrontList* front_list = NULL, + unsigned int qsize = 2); +} // namespace coal /// @endcond diff --git a/src/collision_object.cpp b/src/collision_object.cpp index 1b0ec1b71..a0501b146 100644 --- a/src/collision_object.cpp +++ b/src/collision_object.cpp @@ -35,13 +35,10 @@ /** \author Florent Lamiraux */ -#include +#include "coal/collision_object.h" -namespace hpp { -namespace fcl { +namespace coal { bool CollisionGeometry::isUncertain() const { return !isOccupied() && !isFree(); } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/collision_utility.cpp b/src/collision_utility.cpp index 05fd42969..52839d100 100644 --- a/src/collision_utility.cpp +++ b/src/collision_utility.cpp @@ -1,35 +1,33 @@ // Copyright (c) 2017, Joseph Mirabel // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // -// This file is part of hpp-fcl. -// hpp-fcl is free software: you can redistribute it +// This file is part of Coal. +// Coal is free software: you can redistribute it // and/or modify it under the terms of the GNU Lesser General Public // License as published by the Free Software Foundation, either version // 3 of the License, or (at your option) any later version. // -// hpp-fcl is distributed in the hope that it will be +// Coal is distributed in the hope that it will be // useful, but WITHOUT ANY WARRANTY; without even the implied warranty // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Lesser Public License for more details. You should have // received a copy of the GNU Lesser General Public License along with -// hpp-fcl. If not, see . +// Coal. If not, see . -#include +#include "coal/collision_utility.h" +#include "coal/BVH/BVH_utility.h" -#include - -namespace hpp { -namespace fcl { +namespace coal { namespace details { template inline CollisionGeometry* extractBVHtpl(const CollisionGeometry* model, - const Transform3f& pose, + const Transform3s& pose, const AABB& aabb) { // Ensure AABB is already computed if (model->aabb_radius < 0) - HPP_FCL_THROW_PRETTY("Collision geometry AABB should be computed first.", - std::invalid_argument); + COAL_THROW_PRETTY("Collision geometry AABB should be computed first.", + std::invalid_argument); AABB objAabb = rotate(translate(model->aabb_local, pose.getTranslation()), pose.getRotation()); if (!objAabb.overlap(aabb)) { @@ -41,7 +39,7 @@ inline CollisionGeometry* extractBVHtpl(const CollisionGeometry* model, } CollisionGeometry* extractBVH(const CollisionGeometry* model, - const Transform3f& pose, const AABB& aabb) { + const Transform3s& pose, const AABB& aabb) { switch (model->getNodeType()) { case BV_AABB: return extractBVHtpl(model, pose, aabb); @@ -60,24 +58,20 @@ CollisionGeometry* extractBVH(const CollisionGeometry* model, case BV_KDOP24: return extractBVHtpl >(model, pose, aabb); default: - HPP_FCL_THROW_PRETTY("Unknown type of bounding volume", - std::runtime_error); + COAL_THROW_PRETTY("Unknown type of bounding volume", std::runtime_error); } } } // namespace details CollisionGeometry* extract(const CollisionGeometry* model, - const Transform3f& pose, const AABB& aabb) { + const Transform3s& pose, const AABB& aabb) { switch (model->getObjectType()) { case OT_BVH: return details::extractBVH(model, pose, aabb); // case OT_GEOM: return model; default: - HPP_FCL_THROW_PRETTY( - "Extraction is not implemented for this type of object", - std::runtime_error); + COAL_THROW_PRETTY("Extraction is not implemented for this type of object", + std::runtime_error); } } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/contact_patch.cpp b/src/contact_patch.cpp index de46983ef..bfc8c7c7d 100644 --- a/src/contact_patch.cpp +++ b/src/contact_patch.cpp @@ -34,19 +34,18 @@ /** \author Louis Montaut */ -#include "hpp/fcl/contact_patch.h" -#include "hpp/fcl/collision_utility.h" +#include "coal/contact_patch.h" +#include "coal/collision_utility.h" -namespace hpp { -namespace fcl { +namespace coal { ContactPatchFunctionMatrix& getContactPatchFunctionLookTable() { static ContactPatchFunctionMatrix table; return table; } -void computeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +void computeContactPatch(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result) { @@ -70,12 +69,12 @@ void computeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1, if (object_type1 == OT_GEOM && (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) { if (!looktable.contact_patch_matrix[node_type2][node_type1]) { - HPP_FCL_THROW_PRETTY("Computing contact patches between node type " - << std::string(get_node_type_name(node_type1)) - << " and node type " - << std::string(get_node_type_name(node_type2)) - << " is not yet supported.", - std::invalid_argument); + COAL_THROW_PRETTY("Computing contact patches between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); } looktable.contact_patch_matrix[node_type2][node_type1]( o2, tf2, o1, tf1, collision_result, &csolver, request, result); @@ -84,12 +83,12 @@ void computeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1, } if (!looktable.contact_patch_matrix[node_type1][node_type2]) { - HPP_FCL_THROW_PRETTY("Contact patch computation between node type " - << std::string(get_node_type_name(node_type1)) - << " and node type " - << std::string(get_node_type_name(node_type2)) - << " is not yet supported.", - std::invalid_argument); + COAL_THROW_PRETTY("Contact patch computation between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); } return looktable.contact_patch_matrix[node_type1][node_type2]( @@ -122,12 +121,12 @@ ComputeContactPatch::ComputeContactPatch(const CollisionGeometry* o1, !looktable.contact_patch_matrix[node_type2][node_type1]) || (!this->swap_geoms && !looktable.contact_patch_matrix[node_type1][node_type2])) { - HPP_FCL_THROW_PRETTY("Collision function between node type " - << std::string(get_node_type_name(node_type1)) - << " and node type " - << std::string(get_node_type_name(node_type2)) - << " is not yet supported.", - std::invalid_argument); + COAL_THROW_PRETTY("Collision function between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); } if (this->swap_geoms) { @@ -137,7 +136,7 @@ ComputeContactPatch::ComputeContactPatch(const CollisionGeometry* o1, } } -void ComputeContactPatch::run(const Transform3f& tf1, const Transform3f& tf2, +void ComputeContactPatch::run(const Transform3s& tf1, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result) const { @@ -158,8 +157,8 @@ void ComputeContactPatch::run(const Transform3f& tf1, const Transform3f& tf2, } } -void ComputeContactPatch::operator()(const Transform3f& tf1, - const Transform3f& tf2, +void ComputeContactPatch::operator()(const Transform3s& tf1, + const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result) const @@ -169,5 +168,4 @@ void ComputeContactPatch::operator()(const Transform3f& tf1, this->run(tf1, tf2, collision_result, request, result); } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/contact_patch/contact_patch_solver.cpp b/src/contact_patch/contact_patch_solver.cpp index 9593c5cd5..bf6418d51 100644 --- a/src/contact_patch/contact_patch_solver.cpp +++ b/src/contact_patch/contact_patch_solver.cpp @@ -34,10 +34,9 @@ /** \authors Louis Montaut */ -#include "hpp/fcl/contact_patch/contact_patch_solver.h" +#include "coal/contact_patch/contact_patch_solver.h" -namespace hpp { -namespace fcl { +namespace coal { namespace details { @@ -47,7 +46,7 @@ template (shape); getShapeSupportSet<_SupportOptions>(shape_, support_set, hint, support_data, num_sampled_supports, tol); @@ -99,9 +98,8 @@ ContactPatchSolver::makeSupportSetFunction(const ShapeBase* shape, } } default: - HPP_FCL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error); + COAL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error); } } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/contact_patch_func_matrix.cpp b/src/contact_patch_func_matrix.cpp index 8357702a6..1a6ad2106 100644 --- a/src/contact_patch_func_matrix.cpp +++ b/src/contact_patch_func_matrix.cpp @@ -34,28 +34,26 @@ /** \author Louis Montaut */ -#include "hpp/fcl/contact_patch_func_matrix.h" -#include "hpp/fcl/shape/geometric_shapes.h" -#include "hpp/fcl/internal/shape_shape_contact_patch_func.h" +#include "coal/contact_patch_func_matrix.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/internal/shape_shape_contact_patch_func.h" +#include "coal/BV/BV.h" -#include "hpp/fcl/BV/BV.h" - -namespace hpp { -namespace fcl { +namespace coal { template struct BVHShapeComputeContactPatch { - static void run(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, + static void run(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchSolver* csolver, const ContactPatchRequest& request, ContactPatchResult& result) { - HPP_FCL_UNUSED_VARIABLE(o1); - HPP_FCL_UNUSED_VARIABLE(tf1); - HPP_FCL_UNUSED_VARIABLE(o2); - HPP_FCL_UNUSED_VARIABLE(tf2); - HPP_FCL_UNUSED_VARIABLE(csolver); + COAL_UNUSED_VARIABLE(o1); + COAL_UNUSED_VARIABLE(tf1); + COAL_UNUSED_VARIABLE(o2); + COAL_UNUSED_VARIABLE(tf2); + COAL_UNUSED_VARIABLE(csolver); for (size_t i = 0; i < collision_result.numContacts(); ++i) { if (i >= request.max_num_patch) { break; @@ -70,17 +68,17 @@ struct BVHShapeComputeContactPatch { template struct HeightFieldShapeComputeContactPatch { - static void run(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, + static void run(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchSolver* csolver, const ContactPatchRequest& request, ContactPatchResult& result) { - HPP_FCL_UNUSED_VARIABLE(o1); - HPP_FCL_UNUSED_VARIABLE(tf1); - HPP_FCL_UNUSED_VARIABLE(o2); - HPP_FCL_UNUSED_VARIABLE(tf2); - HPP_FCL_UNUSED_VARIABLE(csolver); + COAL_UNUSED_VARIABLE(o1); + COAL_UNUSED_VARIABLE(tf1); + COAL_UNUSED_VARIABLE(o2); + COAL_UNUSED_VARIABLE(tf2); + COAL_UNUSED_VARIABLE(csolver); for (size_t i = 0; i < collision_result.numContacts(); ++i) { if (i >= request.max_num_patch) { break; @@ -95,17 +93,17 @@ struct HeightFieldShapeComputeContactPatch { template struct BVHComputeContactPatch { - static void run(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, + static void run(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchSolver* csolver, const ContactPatchRequest& request, ContactPatchResult& result) { - HPP_FCL_UNUSED_VARIABLE(o1); - HPP_FCL_UNUSED_VARIABLE(tf1); - HPP_FCL_UNUSED_VARIABLE(o2); - HPP_FCL_UNUSED_VARIABLE(tf2); - HPP_FCL_UNUSED_VARIABLE(csolver); + COAL_UNUSED_VARIABLE(o1); + COAL_UNUSED_VARIABLE(tf1); + COAL_UNUSED_VARIABLE(o2); + COAL_UNUSED_VARIABLE(tf2); + COAL_UNUSED_VARIABLE(csolver); for (size_t i = 0; i < collision_result.numContacts(); ++i) { if (i >= request.max_num_patch) { break; @@ -118,21 +116,21 @@ struct BVHComputeContactPatch { } }; -HPP_FCL_LOCAL void contact_patch_function_not_implemented( - const CollisionGeometry* o1, const Transform3f& /*tf1*/, - const CollisionGeometry* o2, const Transform3f& /*tf2*/, +COAL_LOCAL void contact_patch_function_not_implemented( + const CollisionGeometry* o1, const Transform3s& /*tf1*/, + const CollisionGeometry* o2, const Transform3s& /*tf2*/, const CollisionResult& /*collision_result*/, const ContactPatchSolver* /*csolver*/, const ContactPatchRequest& /*request*/, ContactPatchResult& /*result*/) { NODE_TYPE node_type1 = o1->getNodeType(); NODE_TYPE node_type2 = o2->getNodeType(); - HPP_FCL_THROW_PRETTY("Contact patch function between node type " - << std::string(get_node_type_name(node_type1)) - << " and node type " - << std::string(get_node_type_name(node_type2)) - << " is not yet supported.", - std::invalid_argument); + COAL_THROW_PRETTY("Contact patch function between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); } ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { @@ -364,7 +362,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHComputeContactPatch::run; // TODO(louis): octrees -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP contact_patch_matrix[GEOM_OCTREE][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][GEOM_BOX] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][GEOM_SPHERE] = &contact_patch_function_not_implemented; @@ -411,5 +409,4 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { // clang-format on } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance.cpp b/src/distance.cpp index a234b5ff5..42cc52e09 100644 --- a/src/distance.cpp +++ b/src/distance.cpp @@ -35,31 +35,30 @@ /** \author Jia Pan */ -#include -#include -#include -#include +#include "coal/distance.h" +#include "coal/collision_utility.h" +#include "coal/distance_func_matrix.h" +#include "coal/narrowphase/narrowphase.h" #include -namespace hpp { -namespace fcl { +namespace coal { DistanceFunctionMatrix& getDistanceFunctionLookTable() { static DistanceFunctionMatrix table; return table; } -FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, - const DistanceRequest& request, DistanceResult& result) { +CoalScalar distance(const CollisionObject* o1, const CollisionObject* o2, + const DistanceRequest& request, DistanceResult& result) { return distance(o1->collisionGeometryPtr(), o1->getTransform(), o2->collisionGeometryPtr(), o2->getTransform(), request, result); } -FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const DistanceRequest& request, DistanceResult& result) { +CoalScalar distance(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const DistanceRequest& request, DistanceResult& result) { GJKSolver solver(request); const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable(); @@ -69,17 +68,17 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, OBJECT_TYPE object_type2 = o2->getObjectType(); NODE_TYPE node_type2 = o2->getNodeType(); - FCL_REAL res = (std::numeric_limits::max)(); + CoalScalar res = (std::numeric_limits::max)(); if (object_type1 == OT_GEOM && (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) { if (!looktable.distance_matrix[node_type2][node_type1]) { - HPP_FCL_THROW_PRETTY("Distance function between node type " - << std::string(get_node_type_name(node_type1)) - << " and node type " - << std::string(get_node_type_name(node_type2)) - << " is not yet supported.", - std::invalid_argument); + COAL_THROW_PRETTY("Distance function between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); } else { res = looktable.distance_matrix[node_type2][node_type1]( o2, tf2, o1, tf1, &solver, request, result); @@ -89,12 +88,12 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, } } else { if (!looktable.distance_matrix[node_type1][node_type2]) { - HPP_FCL_THROW_PRETTY("Distance function between node type " - << std::string(get_node_type_name(node_type1)) - << " and node type " - << std::string(get_node_type_name(node_type2)) - << " is not yet supported.", - std::invalid_argument); + COAL_THROW_PRETTY("Distance function between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); } else { res = looktable.distance_matrix[node_type1][node_type2]( o1, tf1, o2, tf2, &solver, request, result); @@ -123,12 +122,12 @@ ComputeDistance::ComputeDistance(const CollisionGeometry* o1, if ((swap_geoms && !looktable.distance_matrix[node_type2][node_type1]) || (!swap_geoms && !looktable.distance_matrix[node_type1][node_type2])) { - HPP_FCL_THROW_PRETTY("Distance function between node type " - << std::string(get_node_type_name(node_type1)) - << " and node type " - << std::string(get_node_type_name(node_type2)) - << " is not yet supported.", - std::invalid_argument); + COAL_THROW_PRETTY("Distance function between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); } if (swap_geoms) func = looktable.distance_matrix[node_type2][node_type1]; @@ -136,10 +135,10 @@ ComputeDistance::ComputeDistance(const CollisionGeometry* o1, func = looktable.distance_matrix[node_type1][node_type2]; } -FCL_REAL ComputeDistance::run(const Transform3f& tf1, const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result) const { - FCL_REAL res; +CoalScalar ComputeDistance::run(const Transform3s& tf1, const Transform3s& tf2, + const DistanceRequest& request, + DistanceResult& result) const { + CoalScalar res; if (swap_geoms) { res = func(o2, tf2, o1, tf1, &solver, request, result); @@ -157,13 +156,13 @@ FCL_REAL ComputeDistance::run(const Transform3f& tf1, const Transform3f& tf2, return res; } -FCL_REAL ComputeDistance::operator()(const Transform3f& tf1, - const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result) const { +CoalScalar ComputeDistance::operator()(const Transform3s& tf1, + const Transform3s& tf2, + const DistanceRequest& request, + DistanceResult& result) const { solver.set(request); - FCL_REAL res; + CoalScalar res; if (request.enable_timings) { Timer timer; res = run(tf1, tf2, request, result); @@ -173,5 +172,4 @@ FCL_REAL ComputeDistance::operator()(const Transform3f& tf1, return res; } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/box_halfspace.cpp b/src/distance/box_halfspace.cpp index 642bd3cb3..913f626e3 100644 --- a/src/distance/box_halfspace.cpp +++ b/src/distance/box_halfspace.cpp @@ -36,36 +36,35 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Box& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Box& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); @@ -73,5 +72,4 @@ FCL_REAL ShapeShapeDistance( } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/box_plane.cpp b/src/distance/box_plane.cpp index fbbffbcf0..b3cb6015e 100644 --- a/src/distance/box_plane.cpp +++ b/src/distance/box_plane.cpp @@ -36,44 +36,42 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver*, const bool, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, + const GJKSolver*, const bool, + Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Box& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver*, const bool, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, + const GJKSolver*, const bool, + Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Box& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/box_sphere.cpp b/src/distance/box_sphere.cpp index cdc8e9495..0b6dbfb24 100644 --- a/src/distance/box_sphere.cpp +++ b/src/distance/box_sphere.cpp @@ -36,44 +36,38 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver*, const bool, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Box& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); return details::boxSphereDistance(s1, tf1, s2, tf2, p1, p2, normal); } template <> -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver*, const bool, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Box& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::boxSphereDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/capsule_capsule.cpp b/src/distance/capsule_capsule.cpp index 635a0c4c7..0ec5137f6 100644 --- a/src/distance/capsule_capsule.cpp +++ b/src/distance/capsule_capsule.cpp @@ -31,9 +31,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/internal/shape_shape_func.h" // Note that partial specialization of template functions is not allowed. // Therefore, two implementations with the default narrow phase solvers are @@ -43,13 +43,12 @@ // // One solution would be to make narrow phase solvers derive from an abstract // class and specialize the template for this abstract class. -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { /// Clamp num / denom in [0, 1] -FCL_REAL clamp(const FCL_REAL& num, const FCL_REAL& denom) { +CoalScalar clamp(const CoalScalar& num, const CoalScalar& denom) { assert(denom >= 0.); if (num <= 0.) return 0.; @@ -60,8 +59,8 @@ FCL_REAL clamp(const FCL_REAL& num, const FCL_REAL& denom) { } /// Clamp s=s_n/s_d in [0, 1] and stores a + s * d in a_sd -void clamped_linear(Vec3f& a_sd, const Vec3f& a, const FCL_REAL& s_n, - const FCL_REAL& s_d, const Vec3f& d) { +void clamped_linear(Vec3s& a_sd, const Vec3s& a, const CoalScalar& s_n, + const CoalScalar& s_d, const Vec3s& d) { assert(s_d >= 0.); if (s_n <= 0.) a_sd = a; @@ -78,42 +77,42 @@ void clamped_linear(Vec3f& a_sd, const Vec3f& a, const FCL_REAL& s_n, /// @param wp1, wp2: witness points on the capsules /// @param normal: normal pointing from capsule1 to capsule2 template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& wp1, Vec3f& wp2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& wp1, Vec3s& wp2, Vec3s& normal) { const Capsule* capsule1 = static_cast(o1); const Capsule* capsule2 = static_cast(o2); - FCL_REAL EPSILON = std::numeric_limits::epsilon() * 100; + CoalScalar EPSILON = std::numeric_limits::epsilon() * 100; // We assume that capsules are centered at the origin. - const fcl::Vec3f& c1 = tf1.getTranslation(); - const fcl::Vec3f& c2 = tf2.getTranslation(); + const coal::Vec3s& c1 = tf1.getTranslation(); + const coal::Vec3s& c2 = tf2.getTranslation(); // We assume that capsules are oriented along z-axis. - FCL_REAL halfLength1 = capsule1->halfLength; - FCL_REAL halfLength2 = capsule2->halfLength; - FCL_REAL radius1 = (capsule1->radius + capsule1->getSweptSphereRadius()); - FCL_REAL radius2 = (capsule2->radius + capsule2->getSweptSphereRadius()); + CoalScalar halfLength1 = capsule1->halfLength; + CoalScalar halfLength2 = capsule2->halfLength; + CoalScalar radius1 = (capsule1->radius + capsule1->getSweptSphereRadius()); + CoalScalar radius2 = (capsule2->radius + capsule2->getSweptSphereRadius()); // direction of capsules // ||d1|| = 2 * halfLength1 - const fcl::Vec3f d1 = 2 * halfLength1 * tf1.getRotation().col(2); - const fcl::Vec3f d2 = 2 * halfLength2 * tf2.getRotation().col(2); + const coal::Vec3s d1 = 2 * halfLength1 * tf1.getRotation().col(2); + const coal::Vec3s d2 = 2 * halfLength2 * tf2.getRotation().col(2); // Starting point of the segments // p1 + d1 is the end point of the segment - const fcl::Vec3f p1 = c1 - d1 / 2; - const fcl::Vec3f p2 = c2 - d2 / 2; - const fcl::Vec3f r = p1 - p2; - FCL_REAL a = d1.dot(d1); - FCL_REAL b = d1.dot(d2); - FCL_REAL c = d1.dot(r); - FCL_REAL e = d2.dot(d2); - FCL_REAL f = d2.dot(r); + const coal::Vec3s p1 = c1 - d1 / 2; + const coal::Vec3s p2 = c2 - d2 / 2; + const coal::Vec3s r = p1 - p2; + CoalScalar a = d1.dot(d1); + CoalScalar b = d1.dot(d2); + CoalScalar c = d1.dot(r); + CoalScalar e = d2.dot(d2); + CoalScalar f = d2.dot(r); // S1 is parametrized by the equation p1 + s * d1 // S2 is parametrized by the equation p2 + t * d2 - Vec3f w1, w2; + Vec3s w1, w2; if (a <= EPSILON) { w1 = p1; if (e <= EPSILON) @@ -128,10 +127,10 @@ FCL_REAL ShapeShapeDistance( w2 = p2; } else { // Always non-negative, equal 0 if the segments are colinear - FCL_REAL denom = fmax(a * e - b * b, 0); + CoalScalar denom = fmax(a * e - b * b, 0); - FCL_REAL s; - FCL_REAL t; + CoalScalar s; + CoalScalar t; if (denom > EPSILON) { s = clamp((b * f - c * e), denom); t = b * s + f; @@ -153,7 +152,7 @@ FCL_REAL ShapeShapeDistance( } // witness points achieving the distance between the two segments - FCL_REAL distance = (w1 - w2).norm(); + CoalScalar distance = (w1 - w2).norm(); // capsule spcecific distance computation distance = distance - (radius1 + radius2); @@ -167,5 +166,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/capsule_halfspace.cpp b/src/distance/capsule_halfspace.cpp index 7e32dcdb5..70c53b918 100644 --- a/src/distance/capsule_halfspace.cpp +++ b/src/distance/capsule_halfspace.cpp @@ -36,40 +36,38 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Capsule& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Capsule& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/capsule_plane.cpp b/src/distance/capsule_plane.cpp index 3d20410ec..27fe19778 100644 --- a/src/distance/capsule_plane.cpp +++ b/src/distance/capsule_plane.cpp @@ -36,40 +36,38 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Capsule& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Capsule& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/cone_halfspace.cpp b/src/distance/cone_halfspace.cpp index b201ce9af..38276b539 100644 --- a/src/distance/cone_halfspace.cpp +++ b/src/distance/cone_halfspace.cpp @@ -36,40 +36,38 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Cone& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Cone& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/cone_plane.cpp b/src/distance/cone_plane.cpp index 83f61b5ee..b26832a96 100644 --- a/src/distance/cone_plane.cpp +++ b/src/distance/cone_plane.cpp @@ -36,44 +36,38 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver*, const bool, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Cone& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver*, const bool, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Cone& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/convex_halfspace.cpp b/src/distance/convex_halfspace.cpp index 5a25fd2ed..bda32965d 100644 --- a/src/distance/convex_halfspace.cpp +++ b/src/distance/convex_halfspace.cpp @@ -34,38 +34,36 @@ /** \author Joseph Mirabel */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const ConvexBase& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const ConvexBase& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/convex_plane.cpp b/src/distance/convex_plane.cpp index e3acc9e2d..c89b57c7f 100644 --- a/src/distance/convex_plane.cpp +++ b/src/distance/convex_plane.cpp @@ -34,38 +34,36 @@ /** \author Louis Montaut */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const ConvexBase& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const ConvexBase& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/cylinder_halfspace.cpp b/src/distance/cylinder_halfspace.cpp index 3d998e8f5..c29e6f7ca 100644 --- a/src/distance/cylinder_halfspace.cpp +++ b/src/distance/cylinder_halfspace.cpp @@ -36,40 +36,38 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Cylinder& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Cylinder& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/cylinder_plane.cpp b/src/distance/cylinder_plane.cpp index 1147d1bda..2d3db927c 100644 --- a/src/distance/cylinder_plane.cpp +++ b/src/distance/cylinder_plane.cpp @@ -36,40 +36,38 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Cylinder& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Cylinder& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/ellipsoid_halfspace.cpp b/src/distance/ellipsoid_halfspace.cpp index 0cd5c09a8..d69e3db97 100644 --- a/src/distance/ellipsoid_halfspace.cpp +++ b/src/distance/ellipsoid_halfspace.cpp @@ -34,40 +34,38 @@ /** \author Louis Montaut */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Ellipsoid& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Ellipsoid& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/ellipsoid_plane.cpp b/src/distance/ellipsoid_plane.cpp index 797d4b424..eae31b844 100644 --- a/src/distance/ellipsoid_plane.cpp +++ b/src/distance/ellipsoid_plane.cpp @@ -34,39 +34,37 @@ /** \author Louis Montaut */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Ellipsoid& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Ellipsoid& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/halfspace_halfspace.cpp b/src/distance/halfspace_halfspace.cpp index 1abdb6fd9..9bad0a596 100644 --- a/src/distance/halfspace_halfspace.cpp +++ b/src/distance/halfspace_halfspace.cpp @@ -34,26 +34,24 @@ /** \author Louis Montaut */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); return details::halfspaceHalfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/halfspace_plane.cpp b/src/distance/halfspace_plane.cpp index 1608b9af1..b0bd3c079 100644 --- a/src/distance/halfspace_plane.cpp +++ b/src/distance/halfspace_plane.cpp @@ -34,39 +34,37 @@ /** \author Louis Montaut */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); return details::halfspacePlaneDistance(s1, tf1, s2, tf2, p1, p2, normal); } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - FCL_REAL distance = + CoalScalar distance = details::halfspacePlaneDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/plane_plane.cpp b/src/distance/plane_plane.cpp index 0d32413ae..b14bb3099 100644 --- a/src/distance/plane_plane.cpp +++ b/src/distance/plane_plane.cpp @@ -34,28 +34,24 @@ /** \author Louis Montaut */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver*, const bool, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); return details::planePlaneDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/sphere_capsule.cpp b/src/distance/sphere_capsule.cpp index c205ad34e..719041b6e 100644 --- a/src/distance/sphere_capsule.cpp +++ b/src/distance/sphere_capsule.cpp @@ -33,35 +33,34 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Capsule& s2 = static_cast(*o2); return details::sphereCapsuleDistance(s1, tf1, s2, tf2, p1, p2, normal); } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Capsule& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::sphereCapsuleDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; @@ -69,5 +68,4 @@ FCL_REAL ShapeShapeDistance( } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/sphere_cylinder.cpp b/src/distance/sphere_cylinder.cpp index e050a3421..743989f19 100644 --- a/src/distance/sphere_cylinder.cpp +++ b/src/distance/sphere_cylinder.cpp @@ -36,41 +36,38 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Cylinder& s2 = static_cast(*o2); return details::sphereCylinderDistance(s1, tf1, s2, tf2, p1, p2, normal); } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Cylinder& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::sphereCylinderDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } } // namespace internal -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/distance/sphere_halfspace.cpp b/src/distance/sphere_halfspace.cpp index 2983ebd43..9a6fbe305 100644 --- a/src/distance/sphere_halfspace.cpp +++ b/src/distance/sphere_halfspace.cpp @@ -36,41 +36,38 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/distance/sphere_plane.cpp b/src/distance/sphere_plane.cpp index 7c95f2312..21438a549 100644 --- a/src/distance/sphere_plane.cpp +++ b/src/distance/sphere_plane.cpp @@ -36,40 +36,38 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/sphere_sphere.cpp b/src/distance/sphere_sphere.cpp index 5f62d317d..ac72a8c74 100644 --- a/src/distance/sphere_sphere.cpp +++ b/src/distance/sphere_sphere.cpp @@ -33,10 +33,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/internal/shape_shape_func.h" +#include "coal/internal/traversal_node_base.h" #include "../narrowphase/details.h" @@ -48,21 +48,19 @@ // // One solution would be to make narrow phase solvers derive from an abstract // class and specialize the template for this abstract class. -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); return details::sphereSphereDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/triangle_halfspace.cpp b/src/distance/triangle_halfspace.cpp index b260cccc2..5f4da4807 100644 --- a/src/distance/triangle_halfspace.cpp +++ b/src/distance/triangle_halfspace.cpp @@ -34,33 +34,32 @@ /** \author Joseph Mirabel */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const TriangleP& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const TriangleP& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); @@ -68,5 +67,4 @@ FCL_REAL ShapeShapeDistance( } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/triangle_plane.cpp b/src/distance/triangle_plane.cpp index b7eeb14f5..87a90ae5d 100644 --- a/src/distance/triangle_plane.cpp +++ b/src/distance/triangle_plane.cpp @@ -34,33 +34,32 @@ /** \author Louis Montaut */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const TriangleP& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const TriangleP& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); @@ -68,5 +67,4 @@ FCL_REAL ShapeShapeDistance( } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/triangle_sphere.cpp b/src/distance/triangle_sphere.cpp index e7af8bfca..2bcb6beb9 100644 --- a/src/distance/triangle_sphere.cpp +++ b/src/distance/triangle_sphere.cpp @@ -34,33 +34,32 @@ /** \author Louis Montaut */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const TriangleP& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::sphereTriangleDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const TriangleP& s2 = static_cast(*o2); return details::sphereTriangleDistance(s1, tf1, s2, tf2, p1, p2, normal); @@ -68,5 +67,4 @@ FCL_REAL ShapeShapeDistance( } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/triangle_triangle.cpp b/src/distance/triangle_triangle.cpp index 2f7302ebc..ccbe2b7f9 100644 --- a/src/distance/triangle_triangle.cpp +++ b/src/distance/triangle_triangle.cpp @@ -34,20 +34,19 @@ /** \author Louis Montaut */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { namespace internal { template <> -FCL_REAL ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* solver, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const GJKSolver* solver, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { // Transform the triangles in world frame const TriangleP& s1 = static_cast(*o1); const TriangleP t1(tf1.transform(s1.a), tf1.transform(s1.b), @@ -61,11 +60,11 @@ FCL_REAL ShapeShapeDistance( // We don't need to take into account swept-sphere radius in GJK iterations; // the result will be corrected after GJK terminates. solver->minkowski_difference - .set<::hpp::fcl::details::SupportOptions::NoSweptSphere>(&t1, &t2); + .set<::coal::details::SupportOptions::NoSweptSphere>(&t1, &t2); solver->gjk.reset(solver->gjk_max_iterations, solver->gjk_tolerance); // Get GJK initial guess - Vec3f guess; + Vec3s guess; if (solver->gjk_initial_guess == GJKInitialGuess::CachedGuess || solver->enable_cached_guess) { guess = solver->cached_guess; @@ -85,10 +84,10 @@ FCL_REAL ShapeShapeDistance( // Retrieve witness points and normal solver->gjk.getWitnessPointsAndNormal(solver->minkowski_difference, p1, p2, normal); - FCL_REAL distance = solver->gjk.distance; + CoalScalar distance = solver->gjk.distance; if (gjk_status == details::GJK::Collision) { - FCL_REAL penetrationDepth = + CoalScalar penetrationDepth = details::computePenetration(t1.a, t1.b, t1.c, t2.a, t2.b, t2.c, normal); distance = -penetrationDepth; } else { @@ -105,5 +104,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp index 4c253cd1f..d9aae3ba3 100644 --- a/src/distance_func_matrix.cpp +++ b/src/distance_func_matrix.cpp @@ -35,24 +35,23 @@ /** \author Jia Pan */ -#include +#include "coal/distance_func_matrix.h" #include <../src/collision_node.h> -#include -#include -#include +#include "coal/internal/shape_shape_func.h" +#include "coal/internal/traversal_node_setup.h" +#include "coal/internal/shape_shape_func.h" #include <../src/traits_traversal.h> -namespace hpp { -namespace fcl { +namespace coal { -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP template -FCL_REAL Distance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, const DistanceRequest& request, - DistanceResult& result) { +CoalScalar Distance(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const GJKSolver* nsolver, const DistanceRequest& request, + DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; typename TraversalTraitsDistance::CollisionTraversal_t node; const TypeA* obj1 = static_cast(o1); @@ -67,61 +66,62 @@ FCL_REAL Distance(const CollisionGeometry* o1, const Transform3f& tf1, #endif -HPP_FCL_LOCAL FCL_REAL distance_function_not_implemented( - const CollisionGeometry* o1, const Transform3f& /*tf1*/, - const CollisionGeometry* o2, const Transform3f& /*tf2*/, +COAL_LOCAL CoalScalar distance_function_not_implemented( + const CollisionGeometry* o1, const Transform3s& /*tf1*/, + const CollisionGeometry* o2, const Transform3s& /*tf2*/, const GJKSolver* /*nsolver*/, const DistanceRequest& /*request*/, DistanceResult& /*result*/) { NODE_TYPE node_type1 = o1->getNodeType(); NODE_TYPE node_type2 = o2->getNodeType(); - HPP_FCL_THROW_PRETTY("Distance function between node type " - << std::string(get_node_type_name(node_type1)) - << " and node type " - << std::string(get_node_type_name(node_type2)) - << " is not yet supported.", - std::invalid_argument); + COAL_THROW_PRETTY("Distance function between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); } template -struct HPP_FCL_LOCAL BVHShapeDistancer{static FCL_REAL distance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +struct COAL_LOCAL BVHShapeDistancer{static CoalScalar distance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result){ if (request.isSatisfied(result)) return result.min_distance; MeshShapeDistanceTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); BVHModel* obj1_tmp = new BVHModel(*obj1); -Transform3f tf1_tmp = tf1; +Transform3s tf1_tmp = tf1; const T_SH* obj2 = static_cast(o2); initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result); -fcl::distance(&node); +::coal::distance(&node); delete obj1_tmp; return result.min_distance; -} // namespace fcl -}; // namespace hpp +} // namespace coal +} +; namespace details { template -FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) { +CoalScalar orientedBVHShapeDistance(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, + const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; OrientedMeshShapeDistanceTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); const T_SH* obj2 = static_cast(o2); initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); - fcl::distance(&node); + ::coal::distance(&node); return result.min_distance; } @@ -129,12 +129,13 @@ FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1, } // namespace details template -struct HPP_FCL_LOCAL BVHShapeDistancer { - static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) { +struct COAL_LOCAL BVHShapeDistancer { + static CoalScalar distance(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) { return details::orientedBVHShapeDistance< MeshShapeDistanceTraversalNodeRSS, RSS, T_SH>( o1, tf1, o2, tf2, nsolver, request, result); @@ -142,12 +143,13 @@ struct HPP_FCL_LOCAL BVHShapeDistancer { }; template -struct HPP_FCL_LOCAL BVHShapeDistancer { - static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) { +struct COAL_LOCAL BVHShapeDistancer { + static CoalScalar distance(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) { return details::orientedBVHShapeDistance< MeshShapeDistanceTraversalNodekIOS, kIOS, T_SH>( o1, tf1, o2, tf2, nsolver, request, result); @@ -155,12 +157,13 @@ struct HPP_FCL_LOCAL BVHShapeDistancer { }; template -struct HPP_FCL_LOCAL BVHShapeDistancer { - static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) { +struct COAL_LOCAL BVHShapeDistancer { + static CoalScalar distance(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) { return details::orientedBVHShapeDistance< MeshShapeDistanceTraversalNodeOBBRSS, OBBRSS, T_SH>( o1, tf1, o2, tf2, nsolver, request, result); @@ -168,18 +171,18 @@ struct HPP_FCL_LOCAL BVHShapeDistancer { }; template -struct HPP_FCL_LOCAL HeightFieldShapeDistancer{static FCL_REAL distance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +struct COAL_LOCAL HeightFieldShapeDistancer{static CoalScalar distance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, - DistanceResult& result){HPP_FCL_UNUSED_VARIABLE(o1); -HPP_FCL_UNUSED_VARIABLE(tf1); -HPP_FCL_UNUSED_VARIABLE(o2); -HPP_FCL_UNUSED_VARIABLE(tf2); -HPP_FCL_UNUSED_VARIABLE(nsolver); -HPP_FCL_UNUSED_VARIABLE(request); + DistanceResult& result){COAL_UNUSED_VARIABLE(o1); +COAL_UNUSED_VARIABLE(tf1); +COAL_UNUSED_VARIABLE(o2); +COAL_UNUSED_VARIABLE(tf2); +COAL_UNUSED_VARIABLE(nsolver); +COAL_UNUSED_VARIABLE(request); // TODO(jcarpent) -HPP_FCL_THROW_PRETTY( +COAL_THROW_PRETTY( "Distance between a height field and a shape is not implemented", std::invalid_argument); // if(request.isSatisfied(result)) return result.min_distance; @@ -197,17 +200,17 @@ return result.min_distance; ; template -FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const DistanceRequest& request, DistanceResult& result) { +CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const DistanceRequest& request, DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; MeshDistanceTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); const BVHModel* obj2 = static_cast*>(o2); BVHModel* obj1_tmp = new BVHModel(*obj1); - Transform3f tf1_tmp = tf1; + Transform3s tf1_tmp = tf1; BVHModel* obj2_tmp = new BVHModel(*obj2); - Transform3f tf2_tmp = tf2; + Transform3s tf2_tmp = tf2; initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result); distance(&node); @@ -219,12 +222,12 @@ FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, namespace details { template -FCL_REAL orientedMeshDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result) { +CoalScalar orientedMeshDistance(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, + const DistanceRequest& request, + DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; OrientedMeshDistanceTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); @@ -239,39 +242,41 @@ FCL_REAL orientedMeshDistance(const CollisionGeometry* o1, } // namespace details template <> -FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result) { +CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const DistanceRequest& request, + DistanceResult& result) { return details::orientedMeshDistance( o1, tf1, o2, tf2, request, result); } template <> -FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result) { +CoalScalar BVHDistance(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, + const DistanceRequest& request, + DistanceResult& result) { return details::orientedMeshDistance( o1, tf1, o2, tf2, request, result); } template <> -FCL_REAL BVHDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result) { +CoalScalar BVHDistance(const CollisionGeometry* o1, + const Transform3s& tf1, + const CollisionGeometry* o2, + const Transform3s& tf2, + const DistanceRequest& request, + DistanceResult& result) { return details::orientedMeshDistance( o1, tf1, o2, tf2, request, result); } template -FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* /*nsolver*/, - const DistanceRequest& request, DistanceResult& result) { +CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const GJKSolver* /*nsolver*/, + const DistanceRequest& request, DistanceResult& result) { return BVHDistance(o1, tf1, o2, tf2, request, result); } @@ -601,7 +606,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance; distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance; -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP distance_matrix[GEOM_OCTREE][GEOM_BOX] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &Distance; @@ -654,6 +659,4 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { #endif } // template struct DistanceFunctionMatrix; -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/hfield.cpp b/src/hfield.cpp index ba771ec65..af5391379 100644 --- a/src/hfield.cpp +++ b/src/hfield.cpp @@ -34,19 +34,18 @@ /** \author Justin Carpentier */ -#include +#include "coal/hfield.h" -#include -#include +#include "coal/BV/BV.h" +#include "coal/shape/convex.h" -#include -#include +#include "coal/internal/BV_splitter.h" +#include "coal/internal/BV_fitter.h" -#include -#include +#include +#include -namespace hpp { -namespace fcl { +namespace coal { template <> NODE_TYPE HeightField::getNodeType() const { @@ -97,6 +96,4 @@ template class HeightField; // template class HeightField; template class HeightField; -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/intersect.cpp b/src/intersect.cpp index ca886a1d8..cd1320dd3 100644 --- a/src/intersect.cpp +++ b/src/intersect.cpp @@ -35,20 +35,20 @@ /** \author Jia Pan */ -#include +#include "coal/internal/intersect.h" +#include "coal/internal/tools.h" + #include #include #include #include -#include -namespace hpp { -namespace fcl { +namespace coal { -bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, - const Vec3f& v3, Vec3f* n, FCL_REAL* t) { - Vec3f n_ = (v2 - v1).cross(v3 - v1); - FCL_REAL norm2 = n_.squaredNorm(); +bool Intersect::buildTrianglePlane(const Vec3s& v1, const Vec3s& v2, + const Vec3s& v3, Vec3s* n, CoalScalar* t) { + Vec3s n_ = (v2 - v1).cross(v3 - v1); + CoalScalar norm2 = n_.squaredNorm(); if (norm2 > 0) { *n = n_ / sqrt(norm2); *t = n->dot(v1); @@ -57,12 +57,12 @@ bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, return false; } -void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, - const Vec3f& B, Vec3f& VEC, Vec3f& X, - Vec3f& Y) { - Vec3f T; - FCL_REAL A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; - Vec3f TMP; +void TriangleDistance::segPoints(const Vec3s& P, const Vec3s& A, const Vec3s& Q, + const Vec3s& B, Vec3s& VEC, Vec3s& X, + Vec3s& Y) { + Vec3s T; + CoalScalar A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; + Vec3s TMP; T = Q - P; A_dot_A = A.dot(A); @@ -74,12 +74,12 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, // t parameterizes ray P,A // u parameterizes ray Q,B - FCL_REAL t, u; + CoalScalar t, u; // compute t for the closest point on ray P,A to // ray Q,B - FCL_REAL denom = A_dot_A * B_dot_B - A_dot_B * A_dot_B; + CoalScalar denom = A_dot_A * B_dot_B - A_dot_B * A_dot_B; t = (A_dot_T * B_dot_B - B_dot_T * A_dot_B) / denom; @@ -153,13 +153,13 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, } } -FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - Vec3f& P, Vec3f& Q) { +CoalScalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3], + Vec3s& P, Vec3s& Q) { // Compute vectors along the 6 sides - Vec3f Sv[3]; - Vec3f Tv[3]; - Vec3f VEC; + Vec3s Sv[3]; + Vec3s Tv[3]; + Vec3s VEC; Sv[0] = S[1] - S[0]; Sv[1] = S[2] - S[1]; @@ -177,8 +177,8 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], // Even if these tests fail, it may be helpful to know the closest // points found, and whether the triangles were shown disjoint - Vec3f V, Z, minP, minQ; - FCL_REAL mindd; + Vec3s V, Z, minP, minQ; + CoalScalar mindd; int shown_disjoint = 0; mindd = (S[0] - T[0]).squaredNorm() + 1; // Set first minimum safely high @@ -190,7 +190,7 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q); V = Q - P; - FCL_REAL dd = V.dot(V); + CoalScalar dd = V.dot(V); // Verify this closest point pair only if the distance // squared is less than the minimum found thus far. @@ -201,13 +201,13 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], mindd = dd; Z = S[(i + 2) % 3] - P; - FCL_REAL a = Z.dot(VEC); + CoalScalar a = Z.dot(VEC); Z = T[(j + 2) % 3] - Q; - FCL_REAL b = Z.dot(VEC); + CoalScalar b = Z.dot(VEC); if ((a <= 0) && (b >= 0)) return dd; - FCL_REAL p = V.dot(VEC); + CoalScalar p = V.dot(VEC); if (a < 0) a = 0; if (b > 0) b = 0; @@ -232,8 +232,8 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], // First check for case 1 - Vec3f Sn; - FCL_REAL Snl; + Vec3s Sn; + CoalScalar Snl; Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle Snl = Sn.dot(Sn); // Compute square of length of normal @@ -243,7 +243,7 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], if (Snl > 1e-15) { // Get projection lengths of T points - Vec3f Tp; + Vec3s Tp; V = S[0] - T[0]; Tp[0] = V.dot(Sn); @@ -300,14 +300,14 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], } } - Vec3f Tn; - FCL_REAL Tnl; + Vec3s Tn; + CoalScalar Tnl; Tn = Tv[0].cross(Tv[1]); Tnl = Tn.dot(Tn); if (Tnl > 1e-15) { - Vec3f Sp; + Vec3s Sp; V = T[0] - S[0]; Sp[0] = V.dot(Tn); @@ -367,12 +367,12 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], return 0; } -FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - Vec3f& P, Vec3f& Q) { - Vec3f S[3]; - Vec3f T[3]; +CoalScalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2, + const Vec3s& S3, const Vec3s& T1, + const Vec3s& T2, const Vec3s& T3, + Vec3s& P, Vec3s& Q) { + Vec3s S[3]; + Vec3s T[3]; S[0] = S1; S[1] = S2; S[2] = S3; @@ -383,10 +383,10 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, return sqrTriDistance(S, T, P, Q); } -FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - const Matrix3f& R, const Vec3f& Tl, - Vec3f& P, Vec3f& Q) { - Vec3f T_transformed[3]; +CoalScalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3], + const Matrix3s& R, const Vec3s& Tl, + Vec3s& P, Vec3s& Q) { + Vec3s T_transformed[3]; T_transformed[0] = R * T[0] + Tl; T_transformed[1] = R * T[1] + Tl; T_transformed[2] = R * T[2] + Tl; @@ -394,10 +394,10 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], return sqrTriDistance(S, T_transformed, P, Q); } -FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - const Transform3f& tf, Vec3f& P, - Vec3f& Q) { - Vec3f T_transformed[3]; +CoalScalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3], + const Transform3s& tf, Vec3s& P, + Vec3s& Q) { + Vec3s T_transformed[3]; T_transformed[0] = tf.transform(T[0]); T_transformed[1] = tf.transform(T[1]); T_transformed[2] = tf.transform(T[2]); @@ -405,39 +405,39 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], return sqrTriDistance(S, T_transformed, P, Q); } -FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - const Matrix3f& R, const Vec3f& Tl, - Vec3f& P, Vec3f& Q) { - Vec3f T1_transformed = R * T1 + Tl; - Vec3f T2_transformed = R * T2 + Tl; - Vec3f T3_transformed = R * T3 + Tl; +CoalScalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2, + const Vec3s& S3, const Vec3s& T1, + const Vec3s& T2, const Vec3s& T3, + const Matrix3s& R, const Vec3s& Tl, + Vec3s& P, Vec3s& Q) { + Vec3s T1_transformed = R * T1 + Tl; + Vec3s T2_transformed = R * T2 + Tl; + Vec3s T3_transformed = R * T3 + Tl; return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); } -FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - const Transform3f& tf, Vec3f& P, - Vec3f& Q) { - Vec3f T1_transformed = tf.transform(T1); - Vec3f T2_transformed = tf.transform(T2); - Vec3f T3_transformed = tf.transform(T3); +CoalScalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2, + const Vec3s& S3, const Vec3s& T1, + const Vec3s& T2, const Vec3s& T3, + const Transform3s& tf, Vec3s& P, + Vec3s& Q) { + Vec3s T1_transformed = tf.transform(T1); + Vec3s T2_transformed = tf.transform(T2); + Vec3s T3_transformed = tf.transform(T3); return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); } -Project::ProjectResult Project::projectLine(const Vec3f& a, const Vec3f& b, - const Vec3f& p) { +Project::ProjectResult Project::projectLine(const Vec3s& a, const Vec3s& b, + const Vec3s& p) { ProjectResult res; - const Vec3f d = b - a; - const FCL_REAL l = d.squaredNorm(); + const Vec3s d = b - a; + const CoalScalar l = d.squaredNorm(); if (l > 0) { - const FCL_REAL t = (p - a).dot(d); + const CoalScalar t = (p - a).dot(d); res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); res.parameterization[0] = 1 - res.parameterization[1]; if (t >= l) { @@ -455,19 +455,19 @@ Project::ProjectResult Project::projectLine(const Vec3f& a, const Vec3f& b, return res; } -Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b, - const Vec3f& c, - const Vec3f& p) { +Project::ProjectResult Project::projectTriangle(const Vec3s& a, const Vec3s& b, + const Vec3s& c, + const Vec3s& p) { ProjectResult res; static const size_t nexti[3] = {1, 2, 0}; - const Vec3f* vt[] = {&a, &b, &c}; - const Vec3f dl[] = {a - b, b - c, c - a}; - const Vec3f& n = dl[0].cross(dl[1]); - const FCL_REAL l = n.squaredNorm(); + const Vec3s* vt[] = {&a, &b, &c}; + const Vec3s dl[] = {a - b, b - c, c - a}; + const Vec3s& n = dl[0].cross(dl[1]); + const CoalScalar l = n.squaredNorm(); if (l > 0) { - FCL_REAL mindist = -1; + CoalScalar mindist = -1; for (size_t i = 0; i < 3; ++i) { if ((*vt[i] - p).dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the @@ -490,9 +490,9 @@ Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b, if (mindist < 0) // the origin project is within the triangle { - FCL_REAL d = (a - p).dot(n); - FCL_REAL s = sqrt(l); - Vec3f p_to_project = n * (d / l); + CoalScalar d = (a - p).dot(n); + CoalScalar s = sqrt(l); + Vec3s p_to_project = n * (d / l); mindist = p_to_project.squaredNorm(); res.encode = 7; // m = 0x111 res.parameterization[0] = dl[1].cross(b - p - p_to_project).norm() / s; @@ -507,17 +507,17 @@ Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b, return res; } -Project::ProjectResult Project::projectTetrahedra(const Vec3f& a, - const Vec3f& b, - const Vec3f& c, - const Vec3f& d, - const Vec3f& p) { +Project::ProjectResult Project::projectTetrahedra(const Vec3s& a, + const Vec3s& b, + const Vec3s& c, + const Vec3s& d, + const Vec3s& p) { ProjectResult res; static const size_t nexti[] = {1, 2, 0}; - const Vec3f* vt[] = {&a, &b, &c, &d}; - const Vec3f dl[3] = {a - d, b - d, c - d}; - FCL_REAL vl = triple(dl[0], dl[1], dl[2]); + const Vec3s* vt[] = {&a, &b, &c, &d}; + const Vec3s dl[3] = {a - d, b - d, c - d}; + CoalScalar vl = triple(dl[0], dl[1], dl[2]); bool ng = (vl * (a - p).dot((b - c).cross(a - b))) <= 0; if (ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng @@ -525,11 +525,11 @@ Project::ProjectResult Project::projectTetrahedra(const Vec3f& a, // does not grow toward the origin (in fact origin is // on the other side of the abc face) { - FCL_REAL mindist = -1; + CoalScalar mindist = -1; for (size_t i = 0; i < 3; ++i) { size_t j = nexti[i]; - FCL_REAL s = vl * (d - p).dot(dl[i].cross(dl[j])); + CoalScalar s = vl * (d - p).dot(dl[i].cross(dl[j])); if (s > 0) // the origin is to the outside part of a triangle face, then // the optimal can only be on the triangle face { @@ -567,15 +567,15 @@ Project::ProjectResult Project::projectTetrahedra(const Vec3f& a, return res; } -Project::ProjectResult Project::projectLineOrigin(const Vec3f& a, - const Vec3f& b) { +Project::ProjectResult Project::projectLineOrigin(const Vec3s& a, + const Vec3s& b) { ProjectResult res; - const Vec3f d = b - a; - const FCL_REAL l = d.squaredNorm(); + const Vec3s d = b - a; + const CoalScalar l = d.squaredNorm(); if (l > 0) { - const FCL_REAL t = -a.dot(d); + const CoalScalar t = -a.dot(d); res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); res.parameterization[0] = 1 - res.parameterization[1]; if (t >= l) { @@ -593,19 +593,19 @@ Project::ProjectResult Project::projectLineOrigin(const Vec3f& a, return res; } -Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a, - const Vec3f& b, - const Vec3f& c) { +Project::ProjectResult Project::projectTriangleOrigin(const Vec3s& a, + const Vec3s& b, + const Vec3s& c) { ProjectResult res; static const size_t nexti[3] = {1, 2, 0}; - const Vec3f* vt[] = {&a, &b, &c}; - const Vec3f dl[] = {a - b, b - c, c - a}; - const Vec3f& n = dl[0].cross(dl[1]); - const FCL_REAL l = n.squaredNorm(); + const Vec3s* vt[] = {&a, &b, &c}; + const Vec3s dl[] = {a - b, b - c, c - a}; + const Vec3s& n = dl[0].cross(dl[1]); + const CoalScalar l = n.squaredNorm(); if (l > 0) { - FCL_REAL mindist = -1; + CoalScalar mindist = -1; for (size_t i = 0; i < 3; ++i) { if (vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the @@ -628,9 +628,9 @@ Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a, if (mindist < 0) // the origin project is within the triangle { - FCL_REAL d = a.dot(n); - FCL_REAL s = sqrt(l); - Vec3f o_to_project = n * (d / l); + CoalScalar d = a.dot(n); + CoalScalar s = sqrt(l); + Vec3s o_to_project = n * (d / l); mindist = o_to_project.squaredNorm(); res.encode = 7; // m = 0x111 res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s; @@ -645,16 +645,16 @@ Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a, return res; } -Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a, - const Vec3f& b, - const Vec3f& c, - const Vec3f& d) { +Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3s& a, + const Vec3s& b, + const Vec3s& c, + const Vec3s& d) { ProjectResult res; static const size_t nexti[] = {1, 2, 0}; - const Vec3f* vt[] = {&a, &b, &c, &d}; - const Vec3f dl[3] = {a - d, b - d, c - d}; - FCL_REAL vl = triple(dl[0], dl[1], dl[2]); + const Vec3s* vt[] = {&a, &b, &c, &d}; + const Vec3s dl[3] = {a - d, b - d, c - d}; + CoalScalar vl = triple(dl[0], dl[1], dl[2]); bool ng = (vl * a.dot((b - c).cross(a - b))) <= 0; if (ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng @@ -662,11 +662,11 @@ Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a, // does not grow toward the origin (in fact origin is // on the other side of the abc face) { - FCL_REAL mindist = -1; + CoalScalar mindist = -1; for (size_t i = 0; i < 3; ++i) { size_t j = nexti[i]; - FCL_REAL s = vl * d.dot(dl[i].cross(dl[j])); + CoalScalar s = vl * d.dot(dl[i].cross(dl[j])); if (s > 0) // the origin is to the outside part of a triangle face, then // the optimal can only be on the triangle face { @@ -704,6 +704,4 @@ Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a, return res; } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/math/transform.cpp b/src/math/transform.cpp index ee6237c17..502b22ab1 100644 --- a/src/math/transform.cpp +++ b/src/math/transform.cpp @@ -35,22 +35,19 @@ /** \author Jia Pan */ -#include +#include "coal/math/transform.h" -namespace hpp { -namespace fcl { +namespace coal { -void relativeTransform(const Transform3f& tf1, const Transform3f& tf2, - Transform3f& tf) { +void relativeTransform(const Transform3s& tf1, const Transform3s& tf2, + Transform3s& tf) { tf = tf1.inverseTimes(tf2); } -void relativeTransform2(const Transform3f& tf1, const Transform3f& tf2, - Transform3f& tf) { - Matrix3f R(tf2.getRotation() * tf1.getRotation().transpose()); - tf = Transform3f(R, tf2.getTranslation() - R * tf1.getTranslation()); +void relativeTransform2(const Transform3s& tf1, const Transform3s& tf2, + Transform3s& tf) { + Matrix3s R(tf2.getRotation() * tf1.getRotation().transpose()); + tf = Transform3s(R, tf2.getTranslation() - R * tf1.getTranslation()); } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/mesh_loader/assimp.cpp b/src/mesh_loader/assimp.cpp index 495ace602..a0012d764 100644 --- a/src/mesh_loader/assimp.cpp +++ b/src/mesh_loader/assimp.cpp @@ -35,11 +35,11 @@ #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) #define nullptr NULL #endif -#include +#include "coal/mesh_loader/assimp.h" // Assimp >= 5.0 is forcing the use of C++11 keywords. A fix has been submitted // https://github.com/assimp/assimp/pull/2758. The next lines fixes the bug for -// current version of hpp-fcl. +// current version of Coal. #include #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) && \ defined(AI_NO_EXCEPT) @@ -54,8 +54,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { namespace internal { @@ -89,11 +88,11 @@ void Loader::load(const std::string& resource_path) { std::string("Could not load resource ") + resource_path + std::string("\n") + importer->GetErrorString() + std::string("\n") + "Hint: the mesh directory may be wrong."); - HPP_FCL_THROW_PRETTY(exception_message.c_str(), std::invalid_argument); + COAL_THROW_PRETTY(exception_message.c_str(), std::invalid_argument); } if (!scene->HasMeshes()) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( (std::string("No meshes found in file ") + resource_path).c_str(), std::invalid_argument); } @@ -107,7 +106,7 @@ void Loader::load(const std::string& resource_path) { * @param[in] vertices_offset Current number of vertices in the model * @param tv Triangles and Vertices of the mesh submodels */ -unsigned recurseBuildMesh(const fcl::Vec3f& scale, const aiScene* scene, +unsigned recurseBuildMesh(const coal::Vec3s& scale, const aiScene* scene, const aiNode* node, unsigned vertices_offset, TriangleAndVertices& tv) { if (!node) return 0; @@ -132,7 +131,7 @@ unsigned recurseBuildMesh(const fcl::Vec3f& scale, const aiScene* scene, aiVector3D p = input_mesh->mVertices[j]; p *= transform; tv.vertices_.push_back( - fcl::Vec3f(p.x * scale[0], p.y * scale[1], p.z * scale[2])); + coal::Vec3s(p.x * scale[0], p.y * scale[1], p.z * scale[2])); } // add the indices @@ -140,9 +139,9 @@ unsigned recurseBuildMesh(const fcl::Vec3f& scale, const aiScene* scene, aiFace& face = input_mesh->mFaces[j]; assert(face.mNumIndices == 3 && "The size of the face is not valid."); tv.triangles_.push_back( - fcl::Triangle(vertices_offset + face.mIndices[0], - vertices_offset + face.mIndices[1], - vertices_offset + face.mIndices[2])); + coal::Triangle(vertices_offset + face.mIndices[0], + vertices_offset + face.mIndices[1], + vertices_offset + face.mIndices[2])); } nbVertices += input_mesh->mNumVertices; @@ -156,11 +155,10 @@ unsigned recurseBuildMesh(const fcl::Vec3f& scale, const aiScene* scene, return nbVertices; } -void buildMesh(const fcl::Vec3f& scale, const aiScene* scene, +void buildMesh(const coal::Vec3s& scale, const aiScene* scene, unsigned vertices_offset, TriangleAndVertices& tv) { recurseBuildMesh(scale, scene, scene->mRootNode, vertices_offset, tv); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/mesh_loader/loader.cpp b/src/mesh_loader/loader.cpp index cc5543e1e..9b38398ba 100644 --- a/src/mesh_loader/loader.cpp +++ b/src/mesh_loader/loader.cpp @@ -34,19 +34,18 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include "coal/mesh_loader/loader.h" +#include "coal/mesh_loader/assimp.h" #include -#ifdef HPP_FCL_HAS_OCTOMAP -#include +#ifdef COAL_HAS_OCTOMAP +#include "coal/octree.h" #endif -#include +#include "coal/BV/BV.h" -namespace hpp { -namespace fcl { +namespace coal { bool CachedMeshLoader::Key::operator<(const CachedMeshLoader::Key& b) const { const CachedMeshLoader::Key& a = *this; for (int i = 0; i < 3; ++i) { @@ -59,14 +58,14 @@ bool CachedMeshLoader::Key::operator<(const CachedMeshLoader::Key& b) const { } template -BVHModelPtr_t _load(const std::string& filename, const Vec3f& scale) { +BVHModelPtr_t _load(const std::string& filename, const Vec3s& scale) { shared_ptr > polyhedron(new BVHModel); loadPolyhedronFromResource(filename, scale, polyhedron); return polyhedron; } BVHModelPtr_t MeshLoader::load(const std::string& filename, - const Vec3f& scale) { + const Vec3s& scale) { switch (bvType_) { case BV_AABB: return _load(filename, scale); @@ -85,24 +84,23 @@ BVHModelPtr_t MeshLoader::load(const std::string& filename, case BV_KDOP24: return _load >(filename, scale); default: - HPP_FCL_THROW_PRETTY("Unhandled bouding volume type.", - std::invalid_argument); + COAL_THROW_PRETTY("Unhandled bouding volume type.", + std::invalid_argument); } } CollisionGeometryPtr_t MeshLoader::loadOctree(const std::string& filename) { -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP shared_ptr octree(new octomap::OcTree(filename)); - return CollisionGeometryPtr_t(new hpp::fcl::OcTree(octree)); + return CollisionGeometryPtr_t(new coal::OcTree(octree)); #else - HPP_FCL_THROW_PRETTY( - "hpp-fcl compiled without OctoMap. Cannot create OcTrees.", - std::logic_error); + COAL_THROW_PRETTY("Coal compiled without OctoMap. Cannot create OcTrees.", + std::logic_error); #endif } BVHModelPtr_t CachedMeshLoader::load(const std::string& filename, - const Vec3f& scale) { + const Vec3s& scale) { Key key(filename, scale); std::time_t mtime = 0; @@ -125,6 +123,4 @@ BVHModelPtr_t CachedMeshLoader::load(const std::string& filename, cache_[key] = val; return geom; } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h index 17d505058..628953b02 100644 --- a/src/narrowphase/details.h +++ b/src/narrowphase/details.h @@ -36,35 +36,34 @@ */ /** \author Jia Pan, Florent Lamiraux */ -#ifndef HPP_FCL_SRC_NARROWPHASE_DETAILS_H -#define HPP_FCL_SRC_NARROWPHASE_DETAILS_H +#ifndef COAL_SRC_NARROWPHASE_DETAILS_H +#define COAL_SRC_NARROWPHASE_DETAILS_H -#include -#include +#include "coal/internal/traversal_node_setup.h" +#include "coal/narrowphase/narrowphase.h" -namespace hpp { -namespace fcl { +namespace coal { namespace details { // Compute the point on a line segment that is the closest point on the // segment to to another point. The code is inspired by the explanation // given by Dan Sunday's page: // http://geomalgorithms.com/a02-_lines.html -static inline void lineSegmentPointClosestToPoint(const Vec3f& p, - const Vec3f& s1, - const Vec3f& s2, Vec3f& sp) { - Vec3f v = s2 - s1; - Vec3f w = p - s1; +static inline void lineSegmentPointClosestToPoint(const Vec3s& p, + const Vec3s& s1, + const Vec3s& s2, Vec3s& sp) { + Vec3s v = s2 - s1; + Vec3s w = p - s1; - FCL_REAL c1 = w.dot(v); - FCL_REAL c2 = v.dot(v); + CoalScalar c1 = w.dot(v); + CoalScalar c2 = v.dot(v); if (c1 <= 0) { sp = s1; } else if (c2 <= c1) { sp = s2; } else { - FCL_REAL b = c1 / c2; - Vec3f Pb = s1 + v * b; + CoalScalar b = c1 / c2; + Vec3s Pb = s1 + v * b; sp = Pb; } } @@ -73,23 +72,25 @@ static inline void lineSegmentPointClosestToPoint(const Vec3f& p, /// @param p2 witness point on the Capsule. /// @param normal pointing from shape 1 to shape 2 (sphere to capsule). /// @return the distance between the two shapes (negative if penetration). -inline FCL_REAL sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1, - const Capsule& s2, const Transform3f& tf2, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { - Vec3f pos1(tf2.transform(Vec3f(0., 0., s2.halfLength))); - Vec3f pos2(tf2.transform(Vec3f(0., 0., -s2.halfLength))); - Vec3f s_c = tf1.getTranslation(); +inline CoalScalar sphereCapsuleDistance(const Sphere& s1, + const Transform3s& tf1, + const Capsule& s2, + const Transform3s& tf2, Vec3s& p1, + Vec3s& p2, Vec3s& normal) { + Vec3s pos1(tf2.transform(Vec3s(0., 0., s2.halfLength))); + Vec3s pos2(tf2.transform(Vec3s(0., 0., -s2.halfLength))); + Vec3s s_c = tf1.getTranslation(); - Vec3f segment_point; + Vec3s segment_point; lineSegmentPointClosestToPoint(s_c, pos1, pos2, segment_point); normal = segment_point - s_c; - FCL_REAL norm(normal.norm()); - FCL_REAL r1 = s1.radius + s1.getSweptSphereRadius(); - FCL_REAL r2 = s2.radius + s2.getSweptSphereRadius(); - FCL_REAL dist = norm - r1 - r2; + CoalScalar norm(normal.norm()); + CoalScalar r1 = s1.radius + s1.getSweptSphereRadius(); + CoalScalar r2 = s2.radius + s2.getSweptSphereRadius(); + CoalScalar dist = norm - r1 - r2; - static const FCL_REAL eps(std::numeric_limits::epsilon()); + static const CoalScalar eps(std::numeric_limits::epsilon()); if (norm > eps) { normal.normalize(); } else { @@ -104,34 +105,35 @@ inline FCL_REAL sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1, /// @param p2 witness point on the Cylinder. /// @param normal pointing from shape 1 to shape 2 (sphere to cylinder). /// @return the distance between the two shapes (negative if penetration). -inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1, - const Cylinder& s2, - const Transform3f& tf2, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { - static const FCL_REAL eps(sqrt(std::numeric_limits::epsilon())); - FCL_REAL r1(s1.radius); - FCL_REAL r2(s2.radius); - FCL_REAL lz2(s2.halfLength); +inline CoalScalar sphereCylinderDistance(const Sphere& s1, + const Transform3s& tf1, + const Cylinder& s2, + const Transform3s& tf2, Vec3s& p1, + Vec3s& p2, Vec3s& normal) { + static const CoalScalar eps(sqrt(std::numeric_limits::epsilon())); + CoalScalar r1(s1.radius); + CoalScalar r2(s2.radius); + CoalScalar lz2(s2.halfLength); // boundaries of the cylinder axis - Vec3f A(tf2.transform(Vec3f(0, 0, -lz2))); - Vec3f B(tf2.transform(Vec3f(0, 0, lz2))); + Vec3s A(tf2.transform(Vec3s(0, 0, -lz2))); + Vec3s B(tf2.transform(Vec3s(0, 0, lz2))); // Position of the center of the sphere - Vec3f S(tf1.getTranslation()); + Vec3s S(tf1.getTranslation()); // axis of the cylinder - Vec3f u(tf2.getRotation().col(2)); + Vec3s u(tf2.getRotation().col(2)); /// @todo a tiny performance improvement could be achieved using the abscissa /// with S as the origin assert((B - A - (s2.halfLength * 2) * u).norm() < eps); - Vec3f AS(S - A); + Vec3s AS(S - A); // abscissa of S on cylinder axis with A as the origin - FCL_REAL s(u.dot(AS)); - Vec3f P(A + s * u); - Vec3f PS(S - P); - FCL_REAL dPS = PS.norm(); + CoalScalar s(u.dot(AS)); + Vec3s P(A + s * u); + Vec3s PS(S - P); + CoalScalar dPS = PS.norm(); // Normal to cylinder axis such that plane (A, u, v) contains sphere // center - Vec3f v(0, 0, 0); - FCL_REAL dist; + Vec3s v(0, 0, 0); + CoalScalar dist; if (dPS > eps) { // S is not on cylinder axis v = (1 / dPS) * PS; @@ -146,8 +148,8 @@ inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1, } else { // closest point on cylinder is on cylinder circle basis p2 = A + r2 * v; - Vec3f Sp2(p2 - S); - FCL_REAL dSp2 = Sp2.norm(); + Vec3s Sp2(p2 - S); + CoalScalar dSp2 = Sp2.norm(); if (dSp2 > eps) { normal = (1 / dSp2) * Sp2; p1 = S + r1 * normal; @@ -179,8 +181,8 @@ inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1, } else { // closest point on cylinder is on cylinder circle basis p2 = B + r2 * v; - Vec3f Sp2(p2 - S); - FCL_REAL dSp2 = Sp2.norm(); + Vec3s Sp2(p2 - S); + CoalScalar dSp2 = Sp2.norm(); if (dSp2 > eps) { normal = (1 / dSp2) * Sp2; p1 = S + r1 * normal; @@ -197,8 +199,8 @@ inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1, } // Take swept-sphere radius into account - const FCL_REAL ssr1 = s1.getSweptSphereRadius(); - const FCL_REAL ssr2 = s2.getSweptSphereRadius(); + const CoalScalar ssr1 = s1.getSweptSphereRadius(); + const CoalScalar ssr2 = s2.getSweptSphereRadius(); if (ssr1 > 0 || ssr2 > 0) { p1 += ssr1 * normal; p2 -= ssr2 * normal; @@ -212,19 +214,19 @@ inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1, /// @param p2 witness point on the second Sphere. /// @param normal pointing from shape 1 to shape 2 (sphere1 to sphere2). /// @return the distance between the two spheres (negative if penetration). -inline FCL_REAL sphereSphereDistance(const Sphere& s1, const Transform3f& tf1, - const Sphere& s2, const Transform3f& tf2, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { - const fcl::Vec3f& center1 = tf1.getTranslation(); - const fcl::Vec3f& center2 = tf2.getTranslation(); - FCL_REAL r1 = (s1.radius + s1.getSweptSphereRadius()); - FCL_REAL r2 = (s2.radius + s2.getSweptSphereRadius()); - - Vec3f c1c2 = center2 - center1; - FCL_REAL cdist = c1c2.norm(); - Vec3f unit(1, 0, 0); - if (cdist > Eigen::NumTraits::epsilon()) unit = c1c2 / cdist; - FCL_REAL dist = cdist - r1 - r2; +inline CoalScalar sphereSphereDistance(const Sphere& s1, const Transform3s& tf1, + const Sphere& s2, const Transform3s& tf2, + Vec3s& p1, Vec3s& p2, Vec3s& normal) { + const coal::Vec3s& center1 = tf1.getTranslation(); + const coal::Vec3s& center2 = tf2.getTranslation(); + CoalScalar r1 = (s1.radius + s1.getSweptSphereRadius()); + CoalScalar r2 = (s2.radius + s2.getSweptSphereRadius()); + + Vec3s c1c2 = center2 - center1; + CoalScalar cdist = c1c2.norm(); + Vec3s unit(1, 0, 0); + if (cdist > Eigen::NumTraits::epsilon()) unit = c1c2 / cdist; + CoalScalar dist = cdist - r1 - r2; normal = unit; p1.noalias() = center1 + r1 * unit; p2.noalias() = center2 - r2 * unit; @@ -232,14 +234,14 @@ inline FCL_REAL sphereSphereDistance(const Sphere& s1, const Transform3f& tf1, } /** @brief the minimum distance from a point to a line */ -inline FCL_REAL segmentSqrDistance(const Vec3f& from, const Vec3f& to, - const Vec3f& p, Vec3f& nearest) { - Vec3f diff = p - from; - Vec3f v = to - from; - FCL_REAL t = v.dot(diff); +inline CoalScalar segmentSqrDistance(const Vec3s& from, const Vec3s& to, + const Vec3s& p, Vec3s& nearest) { + Vec3s diff = p - from; + Vec3s v = to - from; + CoalScalar t = v.dot(diff); if (t > 0) { - FCL_REAL dotVV = v.squaredNorm(); + CoalScalar dotVV = v.squaredNorm(); if (t < dotVV) { t /= dotVV; diff -= v * t; @@ -255,21 +257,21 @@ inline FCL_REAL segmentSqrDistance(const Vec3f& from, const Vec3f& to, } /// @brief Whether a point's projection is in a triangle -inline bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, - const Vec3f& normal, const Vec3f& p) { - Vec3f edge1(p2 - p1); - Vec3f edge2(p3 - p2); - Vec3f edge3(p1 - p3); +inline bool projectInTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3, + const Vec3s& normal, const Vec3s& p) { + Vec3s edge1(p2 - p1); + Vec3s edge2(p3 - p2); + Vec3s edge3(p1 - p3); - Vec3f p1_to_p(p - p1); - Vec3f p2_to_p(p - p2); - Vec3f p3_to_p(p - p3); + Vec3s p1_to_p(p - p1); + Vec3s p2_to_p(p - p2); + Vec3s p3_to_p(p - p3); - Vec3f edge1_normal(edge1.cross(normal)); - Vec3f edge2_normal(edge2.cross(normal)); - Vec3f edge3_normal(edge3.cross(normal)); + Vec3s edge1_normal(edge1.cross(normal)); + Vec3s edge2_normal(edge2.cross(normal)); + Vec3s edge3_normal(edge3.cross(normal)); - FCL_REAL r1, r2, r3; + CoalScalar r1, r2, r3; r1 = edge1_normal.dot(p1_to_p); r2 = edge2_normal.dot(p2_to_p); r3 = edge3_normal.dot(p3_to_p); @@ -283,30 +285,31 @@ inline bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, /// @param p2 witness point on the second Sphere. /// @param normal pointing from shape 1 to shape 2 (sphere1 to sphere2). /// @return the distance between the two shapes (negative if penetration). -inline FCL_REAL sphereTriangleDistance(const Sphere& s, const Transform3f& tf1, - const TriangleP& tri, - const Transform3f& tf2, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { - const Vec3f& P1 = tf2.transform(tri.a); - const Vec3f& P2 = tf2.transform(tri.b); - const Vec3f& P3 = tf2.transform(tri.c); - - Vec3f tri_normal = (P2 - P1).cross(P3 - P1); +inline CoalScalar sphereTriangleDistance(const Sphere& s, + const Transform3s& tf1, + const TriangleP& tri, + const Transform3s& tf2, Vec3s& p1, + Vec3s& p2, Vec3s& normal) { + const Vec3s& P1 = tf2.transform(tri.a); + const Vec3s& P2 = tf2.transform(tri.b); + const Vec3s& P3 = tf2.transform(tri.c); + + Vec3s tri_normal = (P2 - P1).cross(P3 - P1); tri_normal.normalize(); - const Vec3f& center = tf1.getTranslation(); + const Vec3s& center = tf1.getTranslation(); // Note: comparing an object with a swept-sphere radius of r1 against another // object with a swept-sphere radius of r2 is equivalent to comparing the // first object with a swept-sphere radius of r1 + r2 against the second // object with a swept-sphere radius of 0. - const FCL_REAL& radius = + const CoalScalar& radius = s.radius + s.getSweptSphereRadius() + tri.getSweptSphereRadius(); assert(radius >= 0); assert(s.radius >= 0); - Vec3f p1_to_center = center - P1; - FCL_REAL distance_from_plane = p1_to_center.dot(tri_normal); - Vec3f closest_point( - Vec3f::Constant(std::numeric_limits::quiet_NaN())); - FCL_REAL min_distance_sqr, distance_sqr; + Vec3s p1_to_center = center - P1; + CoalScalar distance_from_plane = p1_to_center.dot(tri_normal); + Vec3s closest_point( + Vec3s::Constant(std::numeric_limits::quiet_NaN())); + CoalScalar min_distance_sqr, distance_sqr; if (distance_from_plane < 0) { distance_from_plane *= -1; @@ -318,7 +321,7 @@ inline FCL_REAL sphereTriangleDistance(const Sphere& s, const Transform3f& tf1, min_distance_sqr = distance_from_plane * distance_from_plane; } else { // Compute distance to each edge and take minimal distance - Vec3f nearest_on_edge; + Vec3s nearest_on_edge; min_distance_sqr = segmentSqrDistance(P1, P2, center, closest_point); distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge); @@ -336,7 +339,7 @@ inline FCL_REAL sphereTriangleDistance(const Sphere& s, const Transform3f& tf1, normal = (closest_point - center).normalized(); p1 = center + normal * (s.radius + s.getSweptSphereRadius()); p2 = closest_point - normal * tri.getSweptSphereRadius(); - const FCL_REAL distance = std::sqrt(min_distance_sqr) - radius; + const CoalScalar distance = std::sqrt(min_distance_sqr) - radius; return distance; } @@ -344,9 +347,9 @@ inline FCL_REAL sphereTriangleDistance(const Sphere& s, const Transform3f& tf1, /// @param p2 closest (or most penetrating) point on the shape, /// @param normal the halfspace normal. /// @return the distance between the two shapes (negative if penetration). -inline FCL_REAL halfspaceDistance(const Halfspace& h, const Transform3f& tf1, - const ShapeBase& s, const Transform3f& tf2, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +inline CoalScalar halfspaceDistance(const Halfspace& h, const Transform3s& tf1, + const ShapeBase& s, const Transform3s& tf2, + Vec3s& p1, Vec3s& p2, Vec3s& normal) { // TODO(louis): handle multiple contact points when the halfspace normal is // parallel to the shape's surface (every primitive except sphere and // ellipsoid). @@ -355,7 +358,7 @@ inline FCL_REAL halfspaceDistance(const Halfspace& h, const Transform3f& tf1, Halfspace new_h = transform(h, tf1); // Express halfspace normal in shape frame - Vec3f n_2(tf2.getRotation().transpose() * new_h.n); + Vec3s n_2(tf2.getRotation().transpose() * new_h.n); // Compute support of shape in direction of halfspace normal int hint = 0; @@ -363,13 +366,13 @@ inline FCL_REAL halfspaceDistance(const Halfspace& h, const Transform3f& tf1, getSupport(&s, -n_2, hint); p2 = tf2.transform(p2); - const FCL_REAL dist = new_h.signedDistance(p2); + const CoalScalar dist = new_h.signedDistance(p2); p1.noalias() = p2 - dist * new_h.n; normal.noalias() = new_h.n; - const FCL_REAL dummy_precision = - std::sqrt(Eigen::NumTraits::dummy_precision()); - HPP_FCL_UNUSED_VARIABLE(dummy_precision); + const CoalScalar dummy_precision = + std::sqrt(Eigen::NumTraits::dummy_precision()); + COAL_UNUSED_VARIABLE(dummy_precision); assert(new_h.distance(p1) <= dummy_precision); return dist; } @@ -378,9 +381,9 @@ inline FCL_REAL halfspaceDistance(const Halfspace& h, const Transform3f& tf1, /// @param p2 closest (or most penetrating) point on the shape, /// @param normal the halfspace normal. /// @return the distance between the two shapes (negative if penetration). -inline FCL_REAL planeDistance(const Plane& plane, const Transform3f& tf1, - const ShapeBase& s, const Transform3f& tf2, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +inline CoalScalar planeDistance(const Plane& plane, const Transform3s& tf1, + const ShapeBase& s, const Transform3s& tf2, + Vec3s& p1, Vec3s& p2, Vec3s& normal) { // TODO(louis): handle multiple contact points when the plane normal is // parallel to the shape's surface (every primitive except sphere and // ellipsoid). @@ -389,28 +392,28 @@ inline FCL_REAL planeDistance(const Plane& plane, const Transform3f& tf1, std::array new_h = transformToHalfspaces(plane, tf1); // Express halfspace normals in shape frame - Vec3f n_h1(tf2.getRotation().transpose() * new_h[0].n); - Vec3f n_h2(tf2.getRotation().transpose() * new_h[1].n); + Vec3s n_h1(tf2.getRotation().transpose() * new_h[0].n); + Vec3s n_h2(tf2.getRotation().transpose() * new_h[1].n); // Compute support of shape in direction of halfspace normal and its opposite int hint = 0; - Vec3f p2h1 = + Vec3s p2h1 = getSupport(&s, -n_h1, hint); p2h1 = tf2.transform(p2h1); hint = 0; - Vec3f p2h2 = + Vec3s p2h2 = getSupport(&s, -n_h2, hint); p2h2 = tf2.transform(p2h2); - FCL_REAL dist1 = new_h[0].signedDistance(p2h1); - FCL_REAL dist2 = new_h[1].signedDistance(p2h2); + CoalScalar dist1 = new_h[0].signedDistance(p2h1); + CoalScalar dist2 = new_h[1].signedDistance(p2h2); - const FCL_REAL dummy_precision = - std::sqrt(Eigen::NumTraits::dummy_precision()); - HPP_FCL_UNUSED_VARIABLE(dummy_precision); + const CoalScalar dummy_precision = + std::sqrt(Eigen::NumTraits::dummy_precision()); + COAL_UNUSED_VARIABLE(dummy_precision); - FCL_REAL dist; + CoalScalar dist; if (dist1 >= dist2) { dist = dist1; p2.noalias() = p2h1; @@ -432,21 +435,21 @@ inline FCL_REAL planeDistance(const Plane& plane, const Transform3f& tf1, /// @param ps the witness point on the sphere. /// @param normal pointing from box to sphere /// @return the distance between the two shapes (negative if penetration). -inline FCL_REAL boxSphereDistance(const Box& b, const Transform3f& tfb, - const Sphere& s, const Transform3f& tfs, - Vec3f& pb, Vec3f& ps, Vec3f& normal) { - const Vec3f& os = tfs.getTranslation(); - const Vec3f& ob = tfb.getTranslation(); - const Matrix3f& Rb = tfb.getRotation(); +inline CoalScalar boxSphereDistance(const Box& b, const Transform3s& tfb, + const Sphere& s, const Transform3s& tfs, + Vec3s& pb, Vec3s& ps, Vec3s& normal) { + const Vec3s& os = tfs.getTranslation(); + const Vec3s& ob = tfb.getTranslation(); + const Matrix3s& Rb = tfb.getRotation(); pb = ob; bool outside = false; - const Vec3f os_in_b_frame(Rb.transpose() * (os - ob)); + const Vec3s os_in_b_frame(Rb.transpose() * (os - ob)); int axis = -1; - FCL_REAL min_d = (std::numeric_limits::max)(); + CoalScalar min_d = (std::numeric_limits::max)(); for (int i = 0; i < 3; ++i) { - FCL_REAL facedist; + CoalScalar facedist; if (os_in_b_frame(i) < -b.halfSide(i)) { // outside pb.noalias() -= b.halfSide(i) * Rb.col(i); outside = true; @@ -463,9 +466,9 @@ inline FCL_REAL boxSphereDistance(const Box& b, const Transform3f& tfb, } } normal = pb - os; - FCL_REAL pdist = normal.norm(); - FCL_REAL dist; // distance between sphere and box - if (outside) { // pb is on the box + CoalScalar pdist = normal.norm(); + CoalScalar dist; // distance between sphere and box + if (outside) { // pb is on the box dist = pdist - s.radius; normal /= -pdist; } else { // pb is inside the box @@ -483,8 +486,8 @@ inline FCL_REAL boxSphereDistance(const Box& b, const Transform3f& tfb, } // Take swept-sphere radius into account - const FCL_REAL ssrb = b.getSweptSphereRadius(); - const FCL_REAL ssrs = s.getSweptSphereRadius(); + const CoalScalar ssrb = b.getSweptSphereRadius(); + const CoalScalar ssrs = s.getSweptSphereRadius(); if (ssrb > 0 || ssrs > 0) { pb += ssrb * normal; ps -= ssrs * normal; @@ -506,36 +509,36 @@ inline FCL_REAL boxSphereDistance(const Box& b, const Transform3f& tfb, /// The points p1 and p2 are the same point and represent the origin of the /// intersection line between the objects. The normal is the direction of this /// line. -inline FCL_REAL halfspaceHalfspaceDistance(const Halfspace& s1, - const Transform3f& tf1, - const Halfspace& s2, - const Transform3f& tf2, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { +inline CoalScalar halfspaceHalfspaceDistance(const Halfspace& s1, + const Transform3s& tf1, + const Halfspace& s2, + const Transform3s& tf2, Vec3s& p1, + Vec3s& p2, Vec3s& normal) { Halfspace new_s1 = transform(s1, tf1); Halfspace new_s2 = transform(s2, tf2); - FCL_REAL distance; - Vec3f dir = (new_s1.n).cross(new_s2.n); - FCL_REAL dir_sq_norm = dir.squaredNorm(); + CoalScalar distance; + Vec3s dir = (new_s1.n).cross(new_s2.n); + CoalScalar dir_sq_norm = dir.squaredNorm(); - if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel + if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel { if (new_s1.n.dot(new_s2.n) > 0) { // If the two halfspaces have the same normal, one is inside the other // and they can't be separated. They have inifinte penetration depth. - distance = -(std::numeric_limits::max)(); + distance = -(std::numeric_limits::max)(); if (new_s1.d <= new_s2.d) { normal = new_s1.n; p1 = normal * distance; p2 = new_s2.n * new_s2.d; assert(new_s2.distance(p2) <= - Eigen::NumTraits::dummy_precision()); + Eigen::NumTraits::dummy_precision()); } else { normal = -new_s1.n; p1 << new_s1.n * new_s1.d; p2 = -(normal * distance); assert(new_s1.distance(p1) <= - Eigen::NumTraits::dummy_precision()); + Eigen::NumTraits::dummy_precision()); } } else { distance = -(new_s1.d + new_s2.d); @@ -547,7 +550,7 @@ inline FCL_REAL halfspaceHalfspaceDistance(const Halfspace& s1, // If the halfspaces are not parallel, they are in collision. // Their distance, in the sens of the norm of separation vector, is infinite // (it's impossible to find a translation which separates them) - distance = -(std::numeric_limits::max)(); + distance = -(std::numeric_limits::max)(); // p1 and p2 are the same point, corresponding to a point on the // intersection line between the two objects. Normal is the direction of // that line. @@ -559,8 +562,8 @@ inline FCL_REAL halfspaceHalfspaceDistance(const Halfspace& s1, } // Take swept-sphere radius into account - const FCL_REAL ssr1 = s1.getSweptSphereRadius(); - const FCL_REAL ssr2 = s2.getSweptSphereRadius(); + const CoalScalar ssr1 = s1.getSweptSphereRadius(); + const CoalScalar ssr2 = s2.getSweptSphereRadius(); if (ssr1 > 0 || ssr2 > 0) { p1 += ssr1 * normal; p2 -= ssr2 * normal; @@ -582,18 +585,19 @@ inline FCL_REAL halfspaceHalfspaceDistance(const Halfspace& s1, /// The points p1 and p2 are the same point and represent the origin of the /// intersection line between the objects. The normal is the direction of this /// line. -inline FCL_REAL halfspacePlaneDistance(const Halfspace& s1, - const Transform3f& tf1, const Plane& s2, - const Transform3f& tf2, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { +inline CoalScalar halfspacePlaneDistance(const Halfspace& s1, + const Transform3s& tf1, + const Plane& s2, + const Transform3s& tf2, Vec3s& p1, + Vec3s& p2, Vec3s& normal) { Halfspace new_s1 = transform(s1, tf1); Plane new_s2 = transform(s2, tf2); - FCL_REAL distance; - Vec3f dir = (new_s1.n).cross(new_s2.n); - FCL_REAL dir_sq_norm = dir.squaredNorm(); + CoalScalar distance; + Vec3s dir = (new_s1.n).cross(new_s2.n); + CoalScalar dir_sq_norm = dir.squaredNorm(); - if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel + if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel { normal = new_s1.n; distance = new_s1.n.dot(new_s2.n) > 0 ? (new_s2.d - new_s1.d) @@ -601,14 +605,14 @@ inline FCL_REAL halfspacePlaneDistance(const Halfspace& s1, p1 = new_s1.n * new_s1.d; p2 = new_s2.n * new_s2.d; assert(new_s1.distance(p1) <= - Eigen::NumTraits::dummy_precision()); + Eigen::NumTraits::dummy_precision()); assert(new_s2.distance(p2) <= - Eigen::NumTraits::dummy_precision()); + Eigen::NumTraits::dummy_precision()); } else { // If the halfspace and plane are not parallel, they are in collision. // Their distance, in the sens of the norm of separation vector, is infinite // (it's impossible to find a translation which separates them) - distance = -(std::numeric_limits::max)(); + distance = -(std::numeric_limits::max)(); // p1 and p2 are the same point, corresponding to a point on the // intersection line between the two objects. Normal is the direction of // that line. @@ -620,8 +624,8 @@ inline FCL_REAL halfspacePlaneDistance(const Halfspace& s1, } // Take swept-sphere radius into account - const FCL_REAL ssr1 = s1.getSweptSphereRadius(); - const FCL_REAL ssr2 = s2.getSweptSphereRadius(); + const CoalScalar ssr1 = s1.getSweptSphereRadius(); + const CoalScalar ssr2 = s2.getSweptSphereRadius(); if (ssr1 > 0 || ssr2 > 0) { p1 += ssr1 * normal; p2 -= ssr2 * normal; @@ -643,27 +647,27 @@ inline FCL_REAL halfspacePlaneDistance(const Halfspace& s1, /// The points p1 and p2 are the same point and represent the origin of the /// intersection line between the objects. The normal is the direction of this /// line. -inline FCL_REAL planePlaneDistance(const Plane& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +inline CoalScalar planePlaneDistance(const Plane& s1, const Transform3s& tf1, + const Plane& s2, const Transform3s& tf2, + Vec3s& p1, Vec3s& p2, Vec3s& normal) { Plane new_s1 = transform(s1, tf1); Plane new_s2 = transform(s2, tf2); - FCL_REAL distance; - Vec3f dir = (new_s1.n).cross(new_s2.n); - FCL_REAL dir_sq_norm = dir.squaredNorm(); + CoalScalar distance; + Vec3s dir = (new_s1.n).cross(new_s2.n); + CoalScalar dir_sq_norm = dir.squaredNorm(); - if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel + if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel { p1 = new_s1.n * new_s1.d; p2 = new_s2.n * new_s2.d; assert(new_s1.distance(p1) <= - Eigen::NumTraits::dummy_precision()); + Eigen::NumTraits::dummy_precision()); assert(new_s2.distance(p2) <= - Eigen::NumTraits::dummy_precision()); + Eigen::NumTraits::dummy_precision()); distance = (p1 - p2).norm(); - if (distance > Eigen::NumTraits::dummy_precision()) { + if (distance > Eigen::NumTraits::dummy_precision()) { normal = (p2 - p1).normalized(); } else { normal = new_s1.n; @@ -672,7 +676,7 @@ inline FCL_REAL planePlaneDistance(const Plane& s1, const Transform3f& tf1, // If the planes are not parallel, they are in collision. // Their distance, in the sens of the norm of separation vector, is infinite // (it's impossible to find a translation which separates them) - distance = -(std::numeric_limits::max)(); + distance = -(std::numeric_limits::max)(); // p1 and p2 are the same point, corresponding to a point on the // intersection line between the two objects. Normal is the direction of // that line. @@ -684,8 +688,8 @@ inline FCL_REAL planePlaneDistance(const Plane& s1, const Transform3f& tf1, } // Take swept-sphere radius into account - const FCL_REAL ssr1 = s1.getSweptSphereRadius(); - const FCL_REAL ssr2 = s2.getSweptSphereRadius(); + const CoalScalar ssr1 = s1.getSweptSphereRadius(); + const CoalScalar ssr2 = s2.getSweptSphereRadius(); if (ssr1 > 0 || ssr2 > 0) { p1 += ssr1 * normal; p2 -= ssr2 * normal; @@ -696,15 +700,15 @@ inline FCL_REAL planePlaneDistance(const Plane& s1, const Transform3f& tf1, } /// See the prototype below -inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2, - const Vec3f& P3, const Vec3f& Q1, - const Vec3f& Q2, const Vec3f& Q3, - Vec3f& normal) { - Vec3f u((P2 - P1).cross(P3 - P1)); +inline CoalScalar computePenetration(const Vec3s& P1, const Vec3s& P2, + const Vec3s& P3, const Vec3s& Q1, + const Vec3s& Q2, const Vec3s& Q3, + Vec3s& normal) { + Vec3s u((P2 - P1).cross(P3 - P1)); normal = u.normalized(); - FCL_REAL depth1((P1 - Q1).dot(normal)); - FCL_REAL depth2((P1 - Q2).dot(normal)); - FCL_REAL depth3((P1 - Q3).dot(normal)); + CoalScalar depth1((P1 - Q1).dot(normal)); + CoalScalar depth2((P1 - Q2).dot(normal)); + CoalScalar depth3((P1 - Q3).dot(normal)); return std::max(depth1, std::max(depth2, depth3)); } @@ -715,23 +719,22 @@ inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2, // // Note that we compute here an upper bound of the penetration distance, // not the exact value. -inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2, - const Vec3f& P3, const Vec3f& Q1, - const Vec3f& Q2, const Vec3f& Q3, - const Transform3f& tf1, - const Transform3f& tf2, Vec3f& normal) { - Vec3f globalP1(tf1.transform(P1)); - Vec3f globalP2(tf1.transform(P2)); - Vec3f globalP3(tf1.transform(P3)); - Vec3f globalQ1(tf2.transform(Q1)); - Vec3f globalQ2(tf2.transform(Q2)); - Vec3f globalQ3(tf2.transform(Q3)); +inline CoalScalar computePenetration(const Vec3s& P1, const Vec3s& P2, + const Vec3s& P3, const Vec3s& Q1, + const Vec3s& Q2, const Vec3s& Q3, + const Transform3s& tf1, + const Transform3s& tf2, Vec3s& normal) { + Vec3s globalP1(tf1.transform(P1)); + Vec3s globalP2(tf1.transform(P2)); + Vec3s globalP3(tf1.transform(P3)); + Vec3s globalQ1(tf2.transform(Q1)); + Vec3s globalQ2(tf2.transform(Q2)); + Vec3s globalQ3(tf2.transform(Q3)); return computePenetration(globalP1, globalP2, globalP3, globalQ1, globalQ2, globalQ3, normal); } } // namespace details -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // HPP_FCL_SRC_NARROWPHASE_DETAILS_H +#endif // COAL_SRC_NARROWPHASE_DETAILS_H diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 3b03a9ff8..0e81fce23 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -36,31 +36,30 @@ /** \author Jia Pan */ -#include -#include -#include -#include -#include -#include +#include "coal/shape/geometric_shapes.h" +#include "coal/narrowphase/gjk.h" +#include "coal/internal/intersect.h" +#include "coal/internal/tools.h" +#include "coal/shape/geometric_shapes_traits.h" +#include "coal/narrowphase/narrowphase_defaults.h" -namespace hpp { -namespace fcl { +namespace coal { namespace details { void GJK::initialize() { - distance_upper_bound = (std::numeric_limits::max)(); + distance_upper_bound = (std::numeric_limits::max)(); gjk_variant = GJKVariant::DefaultGJK; convergence_criterion = GJKConvergenceCriterion::Default; convergence_criterion_type = GJKConvergenceCriterionType::Relative; reset(max_iterations, tolerance); } -void GJK::reset(size_t max_iterations_, FCL_REAL tolerance_) { +void GJK::reset(size_t max_iterations_, CoalScalar tolerance_) { max_iterations = max_iterations_; tolerance = tolerance_; - HPP_FCL_ASSERT(tolerance_ > 0, "Tolerance must be positive.", - std::invalid_argument); + COAL_ASSERT(tolerance_ > 0, "Tolerance must be positive.", + std::invalid_argument); status = DidNotRun; nfree = 0; simplex = nullptr; @@ -68,7 +67,7 @@ void GJK::reset(size_t max_iterations_, FCL_REAL tolerance_) { iterations_momentum_stop = 0; } -Vec3f GJK::getGuessFromSimplex() const { return ray; } +Vec3s GJK::getGuessFromSimplex() const { return ray; } namespace details { @@ -91,7 +90,7 @@ namespace details { // w0 = alpha * w[0].w0 + (1 - alpha) * w[1].w0 // w1 = alpha * w[0].w1 + (1 - alpha) * w[1].w1 // clang-format on -void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) { +void getClosestPoints(const GJK::Simplex& simplex, Vec3s& w0, Vec3s& w1) { GJK::SimplexV* const* vs = simplex.vertex; for (GJK::vertex_id_t i = 0; i < simplex.rank; ++i) { @@ -105,10 +104,10 @@ void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) { w1 = vs[0]->w1; return; case 2: { - const Vec3f &a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1, b = vs[1]->w, + const Vec3s &a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1, b = vs[1]->w, b0 = vs[1]->w0, b1 = vs[1]->w1; - FCL_REAL la, lb; - Vec3f N(b - a); + CoalScalar la, lb; + Vec3s N(b - a); la = N.dot(-a); if (la <= 0) { assert(false); @@ -138,8 +137,8 @@ void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) { vs[2]->w, vs[3]->w); break; default: - HPP_FCL_THROW_PRETTY("The simplex rank must be in [ 1, 4 ]", - std::logic_error); + COAL_THROW_PRETTY("The simplex rank must be in [ 1, 4 ]", + std::logic_error); } w0.setZero(); w1.setZero(); @@ -153,18 +152,18 @@ void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) { /// Inflate the points along a normal. /// The normal is typically the normal of the separating plane found by GJK /// or the normal found by EPA. -/// The normal should follow hpp-fcl convention: it points from shape0 to +/// The normal should follow coal convention: it points from shape0 to /// shape1. template -void inflate(const MinkowskiDiff& shape, const Vec3f& normal, Vec3f& w0, - Vec3f& w1) { +void inflate(const MinkowskiDiff& shape, const Vec3s& normal, Vec3s& w0, + Vec3s& w1) { #ifndef NDEBUG - const FCL_REAL dummy_precision = - Eigen::NumTraits::dummy_precision(); + const CoalScalar dummy_precision = + Eigen::NumTraits::dummy_precision(); assert((normal.norm() - 1) < dummy_precision); #endif - const Eigen::Array& I(shape.swept_sphere_radius); + const Eigen::Array& I(shape.swept_sphere_radius); Eigen::Array inflate(I > 0); if (!inflate.any()) return; @@ -174,10 +173,10 @@ void inflate(const MinkowskiDiff& shape, const Vec3f& normal, Vec3f& w0, } // namespace details -void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0, - Vec3f& w1, Vec3f& normal) const { +void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0, + Vec3s& w1, Vec3s& normal) const { details::getClosestPoints(*simplex, w0, w1); - if ((w1 - w0).norm() > Eigen::NumTraits::dummy_precision()) { + if ((w1 - w0).norm() > Eigen::NumTraits::dummy_precision()) { normal = (w1 - w0).normalized(); } else { normal = -this->ray.normalized(); @@ -185,12 +184,12 @@ void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0, details::inflate(shape, normal, w0, w1); } -GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, +GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3s& guess, const support_func_guess_t& supportHint) { - FCL_REAL alpha = 0; + CoalScalar alpha = 0; iterations = 0; - const FCL_REAL swept_sphere_radius = shape_.swept_sphere_radius.sum(); - const FCL_REAL upper_bound = distance_upper_bound + swept_sphere_radius; + const CoalScalar swept_sphere_radius = shape_.swept_sphere_radius.sum(); + const CoalScalar upper_bound = distance_upper_bound + swept_sphere_radius; free_v[0] = &store_v[0]; free_v[1] = &store_v[1]; @@ -205,19 +204,19 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, simplices[current].rank = 0; support_hint = supportHint; - FCL_REAL rl = guess.norm(); + CoalScalar rl = guess.norm(); if (rl < tolerance) { - ray = Vec3f(-1, 0, 0); + ray = Vec3s(-1, 0, 0); rl = 1; } else ray = guess; // Momentum GJKVariant current_gjk_variant = gjk_variant; - Vec3f w = ray; - Vec3f dir = ray; - Vec3f y; - FCL_REAL momentum; + Vec3s w = ray; + Vec3s dir = ray; + Vec3s y; + CoalScalar momentum; bool normalize_support_direction = shape->normalize_support_direction; do { vertex_id_t next = (vertex_id_t)(1 - current); @@ -252,9 +251,10 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, // Normalize heuristic for collision pairs involving convex but not // strictly-convex shapes This corresponds to most use cases. if (normalize_support_direction) { - momentum = (FCL_REAL(iterations) + 2) / (FCL_REAL(iterations) + 3); + momentum = + (CoalScalar(iterations) + 2) / (CoalScalar(iterations) + 3); y = momentum * ray + (1 - momentum) * w; - FCL_REAL y_norm = y.norm(); + CoalScalar y_norm = y.norm(); // ray is the point of the Minkowski difference which currently the // closest to the origin. Therefore, y.norm() > ray.norm() Hence, if // check A above has not stopped the algorithm, we necessarily have @@ -262,19 +262,20 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, assert(y_norm > tolerance); dir = momentum * dir / dir.norm() + (1 - momentum) * y / y_norm; } else { - momentum = (FCL_REAL(iterations) + 1) / (FCL_REAL(iterations) + 3); + momentum = + (CoalScalar(iterations) + 1) / (CoalScalar(iterations) + 3); y = momentum * ray + (1 - momentum) * w; dir = momentum * dir + (1 - momentum) * y; } break; case PolyakAcceleration: - momentum = 1 / (FCL_REAL(iterations) + 1); + momentum = 1 / (CoalScalar(iterations) + 1); dir = momentum * dir + (1 - momentum) * ray; break; default: - HPP_FCL_THROW_PRETTY("Invalid momentum variant.", std::logic_error); + COAL_THROW_PRETTY("Invalid momentum variant.", std::logic_error); } // see below, ray points away from origin @@ -285,7 +286,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, w = curr_simplex.vertex[curr_simplex.rank - 1]->w; // check B: no collision if omega > 0 - FCL_REAL omega = dir.dot(w) / dir.norm(); + CoalScalar omega = dir.dot(w) / dir.norm(); if (omega > upper_bound) { distance = omega - swept_sphere_radius; status = NoCollisionEarlyStopped; @@ -294,7 +295,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, // Check to remove acceleration if (current_gjk_variant != DefaultGJK) { - FCL_REAL frank_wolfe_duality_gap = 2 * ray.dot(ray - w); + CoalScalar frank_wolfe_duality_gap = 2 * ray.dot(ray - w); if (frank_wolfe_duality_gap - tolerance <= 0) { removeVertex(simplices[current]); current_gjk_variant = DefaultGJK; // move back to classic GJK @@ -346,7 +347,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, inside = projectTetrahedraOrigin(curr_simplex, next_simplex); break; default: - HPP_FCL_THROW_PRETTY("Invalid simplex rank", std::logic_error); + COAL_THROW_PRETTY("Invalid simplex rank", std::logic_error); } assert(nfree + next_simplex.rank == 4); current = next; @@ -369,8 +370,8 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, return status; } -bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, - const FCL_REAL& omega) const { +bool GJK::checkConvergence(const Vec3s& w, const CoalScalar& rl, + CoalScalar& alpha, const CoalScalar& omega) const { // x^* is the optimal solution (projection of origin onto the Minkowski // difference). // x^k is the current iterate (x^k = `ray` in the code). @@ -381,13 +382,13 @@ bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, // alpha is the distance to the best separating hyperplane found so far alpha = std::max(alpha, omega); // ||x^*|| - ||x^k|| <= diff - const FCL_REAL diff = rl - alpha; + const CoalScalar diff = rl - alpha; return ((diff - (tolerance + tolerance * rl)) <= 0); } break; case DualityGap: { // ||x^* - x^k||^2 <= diff - const FCL_REAL diff = 2 * ray.dot(ray - w); + const CoalScalar diff = 2 * ray.dot(ray - w); switch (convergence_criterion_type) { case Absolute: return ((diff - tolerance) <= 0); @@ -396,8 +397,8 @@ bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, return (((diff / tolerance * rl) - tolerance * rl) <= 0); break; default: - HPP_FCL_THROW_PRETTY("Invalid convergence criterion type.", - std::logic_error); + COAL_THROW_PRETTY("Invalid convergence criterion type.", + std::logic_error); } } break; @@ -405,7 +406,7 @@ bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, // alpha is the distance to the best separating hyperplane found so far alpha = std::max(alpha, omega); // ||x^* - x^k||^2 <= diff - const FCL_REAL diff = rl * rl - alpha * alpha; + const CoalScalar diff = rl * rl - alpha * alpha; switch (convergence_criterion_type) { case Absolute: return ((diff - tolerance) <= 0); @@ -414,13 +415,13 @@ bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, return (((diff / tolerance * rl) - tolerance * rl) <= 0); break; default: - HPP_FCL_THROW_PRETTY("Invalid convergence criterion type.", - std::logic_error); + COAL_THROW_PRETTY("Invalid convergence criterion type.", + std::logic_error); } } break; default: - HPP_FCL_THROW_PRETTY("Invalid convergence criterion.", std::logic_error); + COAL_THROW_PRETTY("Invalid convergence criterion.", std::logic_error); } } @@ -428,14 +429,14 @@ inline void GJK::removeVertex(Simplex& simplex) { free_v[nfree++] = simplex.vertex[--simplex.rank]; } -inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v, +inline void GJK::appendVertex(Simplex& simplex, const Vec3s& v, support_func_guess_t& hint) { simplex.vertex[simplex.rank] = free_v[--nfree]; // set the memory getSupport(v, *simplex.vertex[simplex.rank++], hint); } bool GJK::encloseOrigin() { - Vec3f axis(Vec3f::Zero()); + Vec3s axis(Vec3s::Zero()); support_func_guess_t hint = support_func_guess_t::Zero(); switch (simplex->rank) { case 1: @@ -452,10 +453,10 @@ bool GJK::encloseOrigin() { } break; case 2: { - Vec3f d = simplex->vertex[1]->w - simplex->vertex[0]->w; + Vec3s d = simplex->vertex[1]->w - simplex->vertex[0]->w; for (int i = 0; i < 3; ++i) { axis[i] = 1; - Vec3f p = d.cross(axis); + Vec3s p = d.cross(axis); if (!p.isZero()) { appendVertex(*simplex, p, hint); if (encloseOrigin()) return true; @@ -492,7 +493,7 @@ bool GJK::encloseOrigin() { } inline void originToPoint(const GJK::Simplex& current, GJK::vertex_id_t a, - const Vec3f& A, GJK::Simplex& next, Vec3f& ray) { + const Vec3s& A, GJK::Simplex& next, Vec3s& ray) { // A is the closest to the origin ray = A; next.vertex[0] = current.vertex[a]; @@ -500,9 +501,9 @@ inline void originToPoint(const GJK::Simplex& current, GJK::vertex_id_t a, } inline void originToSegment(const GJK::Simplex& current, GJK::vertex_id_t a, - GJK::vertex_id_t b, const Vec3f& A, const Vec3f& B, - const Vec3f& AB, const FCL_REAL& ABdotAO, - GJK::Simplex& next, Vec3f& ray) { + GJK::vertex_id_t b, const Vec3s& A, const Vec3s& B, + const Vec3s& AB, const CoalScalar& ABdotAO, + GJK::Simplex& next, Vec3s& ray) { // ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B ray = AB.dot(B) * A + ABdotAO * B; @@ -516,8 +517,8 @@ inline void originToSegment(const GJK::Simplex& current, GJK::vertex_id_t a, inline bool originToTriangle(const GJK::Simplex& current, GJK::vertex_id_t a, GJK::vertex_id_t b, GJK::vertex_id_t c, - const Vec3f& ABC, const FCL_REAL& ABCdotAO, - GJK::Simplex& next, Vec3f& ray) { + const Vec3s& ABC, const CoalScalar& ABCdotAO, + GJK::Simplex& next, Vec3s& ray) { next.rank = 3; next.vertex[2] = current.vertex[a]; @@ -543,11 +544,11 @@ inline bool originToTriangle(const GJK::Simplex& current, GJK::vertex_id_t a, bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) { const vertex_id_t a = 1, b = 0; // A is the last point we added. - const Vec3f& A = current.vertex[a]->w; - const Vec3f& B = current.vertex[b]->w; + const Vec3s& A = current.vertex[a]->w; + const Vec3s& B = current.vertex[b]->w; - const Vec3f AB = B - A; - const FCL_REAL d = AB.dot(-A); + const Vec3s AB = B - A; + const CoalScalar d = AB.dot(-A); assert(d <= AB.squaredNorm()); if (d == 0) { @@ -571,19 +572,19 @@ bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) { bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) { const vertex_id_t a = 2, b = 1, c = 0; // A is the last point we added. - const Vec3f &A = current.vertex[a]->w, B = current.vertex[b]->w, + const Vec3s &A = current.vertex[a]->w, B = current.vertex[b]->w, C = current.vertex[c]->w; - const Vec3f AB = B - A, AC = C - A, ABC = AB.cross(AC); + const Vec3s AB = B - A, AC = C - A, ABC = AB.cross(AC); - FCL_REAL edgeAC2o = ABC.cross(AC).dot(-A); + CoalScalar edgeAC2o = ABC.cross(AC).dot(-A); if (edgeAC2o >= 0) { - FCL_REAL towardsC = AC.dot(-A); + CoalScalar towardsC = AC.dot(-A); if (towardsC >= 0) { // Region 1 originToSegment(current, a, c, A, C, AC, towardsC, next, ray); free_v[nfree++] = current.vertex[b]; } else { // Region 4 or 5 - FCL_REAL towardsB = AB.dot(-A); + CoalScalar towardsB = AB.dot(-A); if (towardsB < 0) { // Region 5 // A is the closest to the origin originToPoint(current, a, A, next, ray); @@ -593,9 +594,9 @@ bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) { free_v[nfree++] = current.vertex[c]; } } else { - FCL_REAL edgeAB2o = AB.cross(ABC).dot(-A); + CoalScalar edgeAB2o = AB.cross(ABC).dot(-A); if (edgeAB2o >= 0) { // Region 4 or 5 - FCL_REAL towardsB = AB.dot(-A); + CoalScalar towardsB = AB.dot(-A); if (towardsB < 0) { // Region 5 // A is the closest to the origin originToPoint(current, a, A, next, ray); @@ -613,35 +614,35 @@ bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) { bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) { // The code of this function was generated using doc/gjk.py const vertex_id_t a = 3, b = 2, c = 1, d = 0; - const Vec3f& A(current.vertex[a]->w); - const Vec3f& B(current.vertex[b]->w); - const Vec3f& C(current.vertex[c]->w); - const Vec3f& D(current.vertex[d]->w); - const FCL_REAL aa = A.squaredNorm(); - const FCL_REAL da = D.dot(A); - const FCL_REAL db = D.dot(B); - const FCL_REAL dc = D.dot(C); - const FCL_REAL dd = D.dot(D); - const FCL_REAL da_aa = da - aa; - const FCL_REAL ca = C.dot(A); - const FCL_REAL cb = C.dot(B); - const FCL_REAL cc = C.dot(C); - const FCL_REAL& cd = dc; - const FCL_REAL ca_aa = ca - aa; - const FCL_REAL ba = B.dot(A); - const FCL_REAL bb = B.dot(B); - const FCL_REAL& bc = cb; - const FCL_REAL& bd = db; - const FCL_REAL ba_aa = ba - aa; - const FCL_REAL ba_ca = ba - ca; - const FCL_REAL ca_da = ca - da; - const FCL_REAL da_ba = da - ba; - const Vec3f a_cross_b = A.cross(B); - const Vec3f a_cross_c = A.cross(C); - - const FCL_REAL dummy_precision( - 3 * std::sqrt(std::numeric_limits::epsilon())); - HPP_FCL_UNUSED_VARIABLE(dummy_precision); + const Vec3s& A(current.vertex[a]->w); + const Vec3s& B(current.vertex[b]->w); + const Vec3s& C(current.vertex[c]->w); + const Vec3s& D(current.vertex[d]->w); + const CoalScalar aa = A.squaredNorm(); + const CoalScalar da = D.dot(A); + const CoalScalar db = D.dot(B); + const CoalScalar dc = D.dot(C); + const CoalScalar dd = D.dot(D); + const CoalScalar da_aa = da - aa; + const CoalScalar ca = C.dot(A); + const CoalScalar cb = C.dot(B); + const CoalScalar cc = C.dot(C); + const CoalScalar& cd = dc; + const CoalScalar ca_aa = ca - aa; + const CoalScalar ba = B.dot(A); + const CoalScalar bb = B.dot(B); + const CoalScalar& bc = cb; + const CoalScalar& bd = db; + const CoalScalar ba_aa = ba - aa; + const CoalScalar ba_ca = ba - ca; + const CoalScalar ca_da = ca - da; + const CoalScalar da_ba = da - ba; + const Vec3s a_cross_b = A.cross(B); + const Vec3s a_cross_c = A.cross(C); + + const CoalScalar dummy_precision( + 3 * std::sqrt(std::numeric_limits::epsilon())); + COAL_UNUSED_VARIABLE(dummy_precision); #define REGION_INSIDE() \ ray.setZero(); \ @@ -1011,7 +1012,7 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) { void EPA::initialize() { reset(max_iterations, tolerance); } -void EPA::reset(size_t max_iterations_, FCL_REAL tolerance_) { +void EPA::reset(size_t max_iterations_, CoalScalar tolerance_) { max_iterations = max_iterations_; tolerance = tolerance_; // EPA creates only 2 faces and 1 vertex per iteration. @@ -1037,18 +1038,18 @@ void EPA::reset(size_t max_iterations_, FCL_REAL tolerance_) { } bool EPA::getEdgeDist(SimplexFace* face, const SimplexVertex& a, - const SimplexVertex& b, FCL_REAL& dist) { - Vec3f ab = b.w - a.w; - Vec3f n_ab = ab.cross(face->n); - FCL_REAL a_dot_nab = a.w.dot(n_ab); + const SimplexVertex& b, CoalScalar& dist) { + Vec3s ab = b.w - a.w; + Vec3s n_ab = ab.cross(face->n); + CoalScalar a_dot_nab = a.w.dot(n_ab); if (a_dot_nab < 0) // the origin is on the outside part of ab { // following is similar to projectOrigin for two points // however, as we dont need to compute the parameterization, dont need to // compute 0 or 1 - FCL_REAL a_dot_ab = a.w.dot(ab); - FCL_REAL b_dot_ab = b.w.dot(ab); + CoalScalar a_dot_ab = a.w.dot(ab); + CoalScalar b_dot_ab = b.w.dot(ab); if (a_dot_ab > 0) dist = a.w.norm(); @@ -1080,15 +1081,15 @@ EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c, const SimplexVertex& c = sv_store[id_c]; face->n = (b.w - a.w).cross(c.w - a.w); - if (face->n.norm() > Eigen::NumTraits::epsilon()) { + if (face->n.norm() > Eigen::NumTraits::epsilon()) { face->n.normalize(); // If the origin projects outside the face, skip it in the // `findClosestFace` method. // The origin always projects inside the closest face. - FCL_REAL a_dot_nab = a.w.dot((b.w - a.w).cross(face->n)); - FCL_REAL b_dot_nbc = b.w.dot((c.w - b.w).cross(face->n)); - FCL_REAL c_dot_nca = c.w.dot((a.w - c.w).cross(face->n)); + CoalScalar a_dot_nab = a.w.dot((b.w - a.w).cross(face->n)); + CoalScalar b_dot_nbc = b.w.dot((c.w - b.w).cross(face->n)); + CoalScalar c_dot_nca = c.w.dot((a.w - c.w).cross(face->n)); if (a_dot_nab >= -tolerance && // b_dot_nbc >= -tolerance && // c_dot_nca >= -tolerance) { @@ -1097,7 +1098,7 @@ EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c, } else { // We will never check this face, so we don't care about // its true distance to the origin. - face->d = std::numeric_limits::max(); + face->d = std::numeric_limits::max(); face->ignore = true; } @@ -1140,10 +1141,10 @@ EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c, /** @brief Find the best polytope face to split */ EPA::SimplexFace* EPA::findClosestFace() { SimplexFace* minf = hull.root; - FCL_REAL mind = std::numeric_limits::max(); + CoalScalar mind = std::numeric_limits::max(); for (SimplexFace* f = minf; f; f = f->next_face) { if (f->ignore) continue; - FCL_REAL sqd = f->d * f->d; + CoalScalar sqd = f->d * f->d; if (sqd < mind) { minf = f; mind = sqd; @@ -1153,7 +1154,7 @@ EPA::SimplexFace* EPA::findClosestFace() { return minf; } -EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) { +EPA::Status EPA::evaluate(GJK& gjk, const Vec3s& guess) { GJK::Simplex& simplex = *gjk.getSimplex(); support_hint = gjk.support_hint; @@ -1246,8 +1247,8 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) { const SimplexVertex& vf1 = sv_store[closest_face->vertex_id[0]]; const SimplexVertex& vf2 = sv_store[closest_face->vertex_id[1]]; const SimplexVertex& vf3 = sv_store[closest_face->vertex_id[2]]; - FCL_REAL fdist = closest_face->n.dot(w.w - vf1.w); - FCL_REAL wnorm = w.w.norm(); + CoalScalar fdist = closest_face->n.dot(w.w - vf1.w); + CoalScalar wnorm = w.w.norm(); // TODO(louis): we might want to use tol_abs and tol_rel; this might // obfuscate the code for the user though. if (fdist <= tolerance + tolerance * wnorm) { @@ -1304,11 +1305,11 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) { // TODO: define a better normal assert(simplex.rank == 1 && simplex.vertex[0]->w.isZero(gjk.getTolerance())); normal = -guess; - FCL_REAL nl = normal.norm(); + CoalScalar nl = normal.norm(); if (nl > 0) normal /= nl; else - normal = Vec3f(1, 0, 0); + normal = Vec3s(1, 0, 0); depth = 0; result.rank = 1; result.vertex[0] = simplex.vertex[0]; @@ -1407,8 +1408,8 @@ bool EPA::expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e, // recursive nature of `expand`, it is safer to go through the first case. // This is because `expand` can potentially loop indefinitly if the // Minkowski difference is very flat (hence the check above). - const FCL_REAL dummy_precision( - 3 * std::sqrt(std::numeric_limits::epsilon())); + const CoalScalar dummy_precision( + 3 * std::sqrt(std::numeric_limits::epsilon())); const SimplexVertex& vf = sv_store[f->vertex_id[e]]; if (f->n.dot(w.w - vf.w) < dummy_precision) { // case 1: the support point is "below" `f`. @@ -1448,10 +1449,10 @@ bool EPA::expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e, return false; } -void EPA::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0, - Vec3f& w1, Vec3f& normal) const { +void EPA::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0, + Vec3s& w1, Vec3s& normal) const { details::getClosestPoints(result, w0, w1); - if ((w0 - w1).norm() > Eigen::NumTraits::dummy_precision()) { + if ((w0 - w1).norm() > Eigen::NumTraits::dummy_precision()) { if (this->depth >= 0) { // The shapes are in collision. normal = (w0 - w1).normalized(); @@ -1476,24 +1477,24 @@ void ConvexBase::buildSupportWarmStart() { this->support_warm_starts.indices.reserve( ConvexBase::num_support_warm_starts); - Vec3f axiis(0, 0, 0); + Vec3s axiis(0, 0, 0); details::ShapeSupportData support_data; int support_hint = 0; for (int i = 0; i < 3; ++i) { axiis(i) = 1; { - Vec3f support; - hpp::fcl::details::getShapeSupport(this, axiis, support, - support_hint, support_data); + Vec3s support; + coal::details::getShapeSupport(this, axiis, support, support_hint, + support_data); this->support_warm_starts.points.emplace_back(support); this->support_warm_starts.indices.emplace_back(support_hint); } axiis(i) = -1; { - Vec3f support; - hpp::fcl::details::getShapeSupport(this, axiis, support, - support_hint, support_data); + Vec3s support; + coal::details::getShapeSupport(this, axiis, support, support_hint, + support_data); this->support_warm_starts.points.emplace_back(support); this->support_warm_starts.indices.emplace_back(support_hint); } @@ -1501,24 +1502,24 @@ void ConvexBase::buildSupportWarmStart() { axiis(i) = 0; } - std::array eis = {Vec3f(1, 1, 1), // - Vec3f(-1, 1, 1), // - Vec3f(-1, -1, 1), // - Vec3f(1, -1, 1)}; + std::array eis = {Vec3s(1, 1, 1), // + Vec3s(-1, 1, 1), // + Vec3s(-1, -1, 1), // + Vec3s(1, -1, 1)}; for (size_t ei_index = 0; ei_index < 4; ++ei_index) { { - Vec3f support; - hpp::fcl::details::getShapeSupport(this, eis[ei_index], support, - support_hint, support_data); + Vec3s support; + coal::details::getShapeSupport(this, eis[ei_index], support, + support_hint, support_data); this->support_warm_starts.points.emplace_back(support); this->support_warm_starts.indices.emplace_back(support_hint); } { - Vec3f support; - hpp::fcl::details::getShapeSupport(this, -eis[ei_index], support, - support_hint, support_data); + Vec3s support; + coal::details::getShapeSupport(this, -eis[ei_index], support, + support_hint, support_data); this->support_warm_starts.points.emplace_back(support); this->support_warm_starts.indices.emplace_back(support_hint); } @@ -1528,11 +1529,9 @@ void ConvexBase::buildSupportWarmStart() { ConvexBase::num_support_warm_starts || this->support_warm_starts.indices.size() != ConvexBase::num_support_warm_starts) { - HPP_FCL_THROW_PRETTY("Wrong number of support warm starts.", - std::runtime_error); + COAL_THROW_PRETTY("Wrong number of support warm starts.", + std::runtime_error); } } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/narrowphase/minkowski_difference.cpp b/src/narrowphase/minkowski_difference.cpp index 4db64acd1..97c90bb57 100644 --- a/src/narrowphase/minkowski_difference.cpp +++ b/src/narrowphase/minkowski_difference.cpp @@ -36,21 +36,20 @@ /** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ -#include "hpp/fcl/narrowphase/minkowski_difference.h" -#include "hpp/fcl/shape/geometric_shapes_traits.h" +#include "coal/narrowphase/minkowski_difference.h" +#include "coal/shape/geometric_shapes_traits.h" -namespace hpp { -namespace fcl { +namespace coal { namespace details { // ============================================================================ template -void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3f& oR1, - const Vec3f& ot1, const Vec3f& dir, Vec3f& support0, - Vec3f& support1, support_func_guess_t& hint, +void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3s& oR1, + const Vec3s& ot1, const Vec3s& dir, Vec3s& support0, + Vec3s& support1, support_func_guess_t& hint, ShapeSupportData data[2]) { - assert(dir.norm() > Eigen::NumTraits::epsilon()); + assert(dir.norm() > Eigen::NumTraits::epsilon()); getShapeSupport<_SupportOptions>(s0, dir, support0, hint[0], data[0]); if (TransformIsIdentity) { @@ -65,8 +64,8 @@ void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3f& oR1, // ============================================================================ template -void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3f& dir, - Vec3f& support0, Vec3f& support1, +void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3s& dir, + Vec3s& support0, Vec3s& support1, support_func_guess_t& hint, ShapeSupportData data[2]) { getSupportTpl( static_cast(md.shapes[0]), @@ -78,7 +77,7 @@ void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3f& dir, template MinkowskiDiff::GetSupportFunction makeGetSupportFunction1( const ShapeBase* s1, bool identity, - Eigen::Array& swept_sphere_radius, + Eigen::Array& swept_sphere_radius, ShapeSupportData data[2]) { if (_SupportOptions == SupportOptions::WithSweptSphere) { // No need to store the information of swept sphere radius @@ -151,7 +150,7 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction1( } } default: - HPP_FCL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error); + COAL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error); } } @@ -159,7 +158,7 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction1( template MinkowskiDiff::GetSupportFunction makeGetSupportFunction0( const ShapeBase* s0, const ShapeBase* s1, bool identity, - Eigen::Array& swept_sphere_radius, + Eigen::Array& swept_sphere_radius, ShapeSupportData data[2]) { if (_SupportOptions == SupportOptions::WithSweptSphere) { // No need to store the information of swept sphere radius @@ -221,7 +220,7 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction0( break; } default: - HPP_FCL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); + COAL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); } } @@ -253,7 +252,7 @@ bool getNormalizeSupportDirection(const ShapeBase* shape) { return (bool)shape_traits::NeedNesterovNormalizeHeuristic; break; default: - HPP_FCL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); + COAL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); } } @@ -268,7 +267,7 @@ void getNormalizeSupportDirectionFromShapes(const ShapeBase* shape0, // ============================================================================ template void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1, - const Transform3f& tf0, const Transform3f& tf1) { + const Transform3s& tf0, const Transform3s& tf1) { shapes[0] = shape0; shapes[1] = shape1; getNormalizeSupportDirectionFromShapes(shape0, shape1, @@ -284,9 +283,9 @@ void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1, shape0, shape1, identity, swept_sphere_radius, data); } // clang-format off -template void HPP_FCL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*); +template void COAL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*); -template void HPP_FCL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*); +template void COAL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*); // clang-format on // ============================================================================ @@ -304,22 +303,21 @@ void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) { shape0, shape1, true, swept_sphere_radius, data); } // clang-format off -template void HPP_FCL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&); +template void COAL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3s&, const Transform3s&); -template void HPP_FCL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&); +template void COAL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3s&, const Transform3s&); // clang-format on // ============================================================================ // clang-format off -template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support0(const Vec3f&, int&) const; +template Vec3s COAL_DLLAPI MinkowskiDiff::support0(const Vec3s&, int&) const; -template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support0(const Vec3f&, int&) const; +template Vec3s COAL_DLLAPI MinkowskiDiff::support0(const Vec3s&, int&) const; -template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support1(const Vec3f&, int&) const; +template Vec3s COAL_DLLAPI MinkowskiDiff::support1(const Vec3s&, int&) const; -template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support1(const Vec3f&, int&) const; +template Vec3s COAL_DLLAPI MinkowskiDiff::support1(const Vec3s&, int&) const; // clang-format on } // namespace details -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/narrowphase/support_functions.cpp b/src/narrowphase/support_functions.cpp index 3b632abb9..a900999d3 100644 --- a/src/narrowphase/support_functions.cpp +++ b/src/narrowphase/support_functions.cpp @@ -36,12 +36,11 @@ /** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ -#include "hpp/fcl/narrowphase/support_functions.h" +#include "coal/narrowphase/support_functions.h" #include -namespace hpp { -namespace fcl { +namespace coal { namespace details { // ============================================================================ @@ -49,8 +48,8 @@ namespace details { getShapeSupport<_SupportOptions>(static_cast(shape), dir, \ support, hint, support_data) template -Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint) { - Vec3f support; +Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint) { + Vec3s support; ShapeSupportData support_data; switch (shape->getNodeType()) { case GEOM_TRIANGLE: @@ -90,30 +89,29 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint) { // Explicit instantiation // clang-format off -template HPP_FCL_DLLAPI Vec3f getSupport(const ShapeBase*, const Vec3f&, int&); +template COAL_DLLAPI Vec3s getSupport(const ShapeBase*, const Vec3s&, int&); -template HPP_FCL_DLLAPI Vec3f getSupport(const ShapeBase*, const Vec3f&, int&); +template COAL_DLLAPI Vec3s getSupport(const ShapeBase*, const Vec3s&, int&); // clang-format on // ============================================================================ -#define getShapeSupportTplInstantiation(ShapeType) \ - template HPP_FCL_DLLAPI void getShapeSupport( \ - const ShapeType* shape_, const Vec3f& dir, Vec3f& support, int& hint, \ - ShapeSupportData& support_data); \ - \ - template HPP_FCL_DLLAPI void \ - getShapeSupport( \ - const ShapeType* shape_, const Vec3f& dir, Vec3f& support, int& hint, \ +#define getShapeSupportTplInstantiation(ShapeType) \ + template COAL_DLLAPI void getShapeSupport( \ + const ShapeType* shape_, const Vec3s& dir, Vec3s& support, int& hint, \ + ShapeSupportData& support_data); \ + \ + template COAL_DLLAPI void getShapeSupport( \ + const ShapeType* shape_, const Vec3s& dir, Vec3s& support, int& hint, \ ShapeSupportData& support_data); // ============================================================================ template -void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, - Vec3f& support, int& /*unused*/, +void getShapeSupport(const TriangleP* triangle, const Vec3s& dir, + Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/) { - FCL_REAL dota = dir.dot(triangle->a); - FCL_REAL dotb = dir.dot(triangle->b); - FCL_REAL dotc = dir.dot(triangle->c); + CoalScalar dota = dir.dot(triangle->a); + CoalScalar dotb = dir.dot(triangle->b); + CoalScalar dotc = dir.dot(triangle->c); if (dota > dotb) { if (dotc > dota) { support = triangle->c; @@ -132,22 +130,19 @@ void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, support += triangle->getSweptSphereRadius() * dir.normalized(); } } -// clang-format off -getShapeSupportTplInstantiation(TriangleP) - // clang-format on - - // ============================================================================ - template - inline void getShapeSupport(const Box* box, const Vec3f& dir, - Vec3f& support, int& /*unused*/, - ShapeSupportData& /*unused*/) { +getShapeSupportTplInstantiation(TriangleP); + +// ============================================================================ +template +inline void getShapeSupport(const Box* box, const Vec3s& dir, Vec3s& support, + int& /*unused*/, ShapeSupportData& /*unused*/) { // The inflate value is simply to make the specialized functions with box // have a preferred side for edge cases. - static const FCL_REAL inflate = (dir.array() == 0).any() ? 1 + 1e-10 : 1.; - static const FCL_REAL dummy_precision = - Eigen::NumTraits::dummy_precision(); - Vec3f support1 = (dir.array() > dummy_precision).select(box->halfSide, 0); - Vec3f support2 = + static const CoalScalar inflate = (dir.array() == 0).any() ? 1 + 1e-10 : 1.; + static const CoalScalar dummy_precision = + Eigen::NumTraits::dummy_precision(); + Vec3s support1 = (dir.array() > dummy_precision).select(box->halfSide, 0); + Vec3s support2 = (dir.array() < -dummy_precision).select(-inflate * box->halfSide, 0); support.noalias() = support1 + support2; @@ -155,15 +150,13 @@ getShapeSupportTplInstantiation(TriangleP) support += box->getSweptSphereRadius() * dir.normalized(); } } -// clang-format off -getShapeSupportTplInstantiation(Box) - // clang-format on - - // ============================================================================ - template - inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir, - Vec3f& support, int& /*unused*/, - ShapeSupportData& /*unused*/) { +getShapeSupportTplInstantiation(Box); + +// ============================================================================ +template +inline void getShapeSupport(const Sphere* sphere, const Vec3s& dir, + Vec3s& support, int& /*unused*/, + ShapeSupportData& /*unused*/) { if (_SupportOptions == SupportOptions::WithSweptSphere) { support.noalias() = (sphere->radius + sphere->getSweptSphereRadius()) * dir.normalized(); @@ -171,25 +164,23 @@ getShapeSupportTplInstantiation(Box) support.setZero(); } - HPP_FCL_UNUSED_VARIABLE(sphere); - HPP_FCL_UNUSED_VARIABLE(dir); + COAL_UNUSED_VARIABLE(sphere); + COAL_UNUSED_VARIABLE(dir); } -// clang-format off -getShapeSupportTplInstantiation(Sphere) - // clang-format on +getShapeSupportTplInstantiation(Sphere); - // ============================================================================ - template - inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir, - Vec3f& support, int& /*unused*/, - ShapeSupportData& /*unused*/) { - FCL_REAL a2 = ellipsoid->radii[0] * ellipsoid->radii[0]; - FCL_REAL b2 = ellipsoid->radii[1] * ellipsoid->radii[1]; - FCL_REAL c2 = ellipsoid->radii[2] * ellipsoid->radii[2]; +// ============================================================================ +template +inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3s& dir, + Vec3s& support, int& /*unused*/, + ShapeSupportData& /*unused*/) { + CoalScalar a2 = ellipsoid->radii[0] * ellipsoid->radii[0]; + CoalScalar b2 = ellipsoid->radii[1] * ellipsoid->radii[1]; + CoalScalar c2 = ellipsoid->radii[2] * ellipsoid->radii[2]; - Vec3f v(a2 * dir[0], b2 * dir[1], c2 * dir[2]); + Vec3s v(a2 * dir[0], b2 * dir[1], c2 * dir[2]); - FCL_REAL d = std::sqrt(v.dot(dir)); + CoalScalar d = std::sqrt(v.dot(dir)); support = v / d; @@ -197,17 +188,15 @@ getShapeSupportTplInstantiation(Sphere) support += ellipsoid->getSweptSphereRadius() * dir.normalized(); } } -// clang-format off -getShapeSupportTplInstantiation(Ellipsoid) - // clang-format on - - // ============================================================================ - template - inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, - Vec3f& support, int& /*unused*/, - ShapeSupportData& /*unused*/) { - static const FCL_REAL dummy_precision = - Eigen::NumTraits::dummy_precision(); +getShapeSupportTplInstantiation(Ellipsoid); + +// ============================================================================ +template +inline void getShapeSupport(const Capsule* capsule, const Vec3s& dir, + Vec3s& support, int& /*unused*/, + ShapeSupportData& /*unused*/) { + static const CoalScalar dummy_precision = + Eigen::NumTraits::dummy_precision(); support.setZero(); if (dir[2] > dummy_precision) { support[2] = capsule->halfLength; @@ -220,23 +209,21 @@ getShapeSupportTplInstantiation(Ellipsoid) (capsule->radius + capsule->getSweptSphereRadius()) * dir.normalized(); } } -// clang-format off -getShapeSupportTplInstantiation(Capsule) - // clang-format on +getShapeSupportTplInstantiation(Capsule); - // ============================================================================ - template - void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, - int& /*unused*/, ShapeSupportData& /*unused*/) { - static const FCL_REAL dummy_precision = - Eigen::NumTraits::dummy_precision(); +// ============================================================================ +template +void getShapeSupport(const Cone* cone, const Vec3s& dir, Vec3s& support, + int& /*unused*/, ShapeSupportData& /*unused*/) { + static const CoalScalar dummy_precision = + Eigen::NumTraits::dummy_precision(); // The cone radius is, for -h < z < h, (h - z) * r / (2*h) // The inflate value is simply to make the specialized functions with cone // have a preferred side for edge cases. - static const FCL_REAL inflate = 1 + 1e-10; - FCL_REAL h = cone->halfLength; - FCL_REAL r = cone->radius; + static const CoalScalar inflate = 1 + 1e-10; + CoalScalar h = cone->halfLength; + CoalScalar r = cone->radius; if (dir.head<2>().isZero(dummy_precision)) { support.head<2>().setZero(); @@ -246,22 +233,22 @@ getShapeSupportTplInstantiation(Capsule) support[2] = -inflate * h; } } else { - FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1]; - FCL_REAL len = zdist + dir[2] * dir[2]; + CoalScalar zdist = dir[0] * dir[0] + dir[1] * dir[1]; + CoalScalar len = zdist + dir[2] * dir[2]; zdist = std::sqrt(zdist); if (dir[2] <= 0) { - FCL_REAL rad = r / zdist; + CoalScalar rad = r / zdist; support.head<2>() = rad * dir.head<2>(); support[2] = -h; } else { len = std::sqrt(len); - FCL_REAL sin_a = r / std::sqrt(r * r + 4 * h * h); + CoalScalar sin_a = r / std::sqrt(r * r + 4 * h * h); if (dir[2] > len * sin_a) support << 0, 0, h; else { - FCL_REAL rad = r / zdist; + CoalScalar rad = r / zdist; support.head<2>() = rad * dir.head<2>(); support[2] = -h; } @@ -272,23 +259,20 @@ getShapeSupportTplInstantiation(Capsule) support += cone->getSweptSphereRadius() * dir.normalized(); } } -// clang-format off -getShapeSupportTplInstantiation(Cone) - // clang-format on +getShapeSupportTplInstantiation(Cone); - // ============================================================================ - template - void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, - Vec3f& support, int& /*unused*/, - ShapeSupportData& /*unused*/) { - static const FCL_REAL dummy_precision = - Eigen::NumTraits::dummy_precision(); +// ============================================================================ +template +void getShapeSupport(const Cylinder* cylinder, const Vec3s& dir, Vec3s& support, + int& /*unused*/, ShapeSupportData& /*unused*/) { + static const CoalScalar dummy_precision = + Eigen::NumTraits::dummy_precision(); // The inflate value is simply to make the specialized functions with cylinder // have a preferred side for edge cases. - static const FCL_REAL inflate = 1 + 1e-10; - FCL_REAL half_h = cylinder->halfLength; - FCL_REAL r = cylinder->radius; + static const CoalScalar inflate = 1 + 1e-10; + CoalScalar half_h = cylinder->halfLength; + CoalScalar r = cylinder->radius; const bool dir_is_aligned_with_z = dir.head<2>().isZero(dummy_precision); if (dir_is_aligned_with_z) half_h *= inflate; @@ -309,35 +293,33 @@ getShapeSupportTplInstantiation(Cone) } assert(fabs(support[0] * dir[1] - support[1] * dir[0]) < - sqrt(std::numeric_limits::epsilon())); + sqrt(std::numeric_limits::epsilon())); if (_SupportOptions == SupportOptions::WithSweptSphere) { support += cylinder->getSweptSphereRadius() * dir.normalized(); } } -// clang-format off -getShapeSupportTplInstantiation(Cylinder) - // clang-format on +getShapeSupportTplInstantiation(Cylinder); - // ============================================================================ - template - void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, - Vec3f& support, int& hint, - ShapeSupportData& support_data) { +// ============================================================================ +template +void getShapeSupportLog(const ConvexBase* convex, const Vec3s& dir, + Vec3s& support, int& hint, + ShapeSupportData& support_data) { assert(convex->neighbors != nullptr && "Convex has no neighbors."); // Use warm start if current support direction is distant from last support // direction. const double use_warm_start_threshold = 0.9; - Vec3f dir_normalized = dir.normalized(); + Vec3s dir_normalized = dir.normalized(); if (!support_data.last_dir.isZero() && !convex->support_warm_starts.points.empty() && support_data.last_dir.dot(dir_normalized) < use_warm_start_threshold) { // Change hint if last dir is too far from current dir. - FCL_REAL maxdot = convex->support_warm_starts.points[0].dot(dir); + CoalScalar maxdot = convex->support_warm_starts.points[0].dot(dir); hint = convex->support_warm_starts.indices[0]; for (size_t i = 1; i < convex->support_warm_starts.points.size(); ++i) { - FCL_REAL dot = convex->support_warm_starts.points[i].dot(dir); + CoalScalar dot = convex->support_warm_starts.points[i].dot(dir); if (dot > maxdot) { maxdot = dot; hint = convex->support_warm_starts.indices[i]; @@ -346,13 +328,13 @@ getShapeSupportTplInstantiation(Cylinder) } support_data.last_dir = dir_normalized; - const std::vector& pts = *(convex->points); + const std::vector& pts = *(convex->points); const std::vector& nn = *(convex->neighbors); if (hint < 0 || hint >= (int)convex->num_points) { hint = 0; } - FCL_REAL maxdot = pts[static_cast(hint)].dot(dir); + CoalScalar maxdot = pts[static_cast(hint)].dot(dir); std::vector& visited = support_data.visited; if (support_data.visited.size() == convex->num_points) { std::fill(visited.begin(), visited.end(), false); @@ -374,7 +356,7 @@ getShapeSupportTplInstantiation(Cylinder) const unsigned int ip = n[in]; if (visited[ip]) continue; visited[ip] = true; - const FCL_REAL dot = pts[ip].dot(dir); + const CoalScalar dot = pts[ip].dot(dir); bool better = false; if (dot > maxdot) { better = true; @@ -398,15 +380,15 @@ getShapeSupportTplInstantiation(Cylinder) // ============================================================================ template -void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir, - Vec3f& support, int& hint, +void getShapeSupportLinear(const ConvexBase* convex, const Vec3s& dir, + Vec3s& support, int& hint, ShapeSupportData& /*unused*/) { - const std::vector& pts = *(convex->points); + const std::vector& pts = *(convex->points); hint = 0; - FCL_REAL maxdot = pts[0].dot(dir); + CoalScalar maxdot = pts[0].dot(dir); for (int i = 1; i < (int)convex->num_points; ++i) { - FCL_REAL dot = pts[static_cast(i)].dot(dir); + CoalScalar dot = pts[static_cast(i)].dot(dir); if (dot > maxdot) { maxdot = dot; hint = i; @@ -422,7 +404,7 @@ void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir, // ============================================================================ template -void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, +void getShapeSupport(const ConvexBase* convex, const Vec3s& dir, Vec3s& support, int& hint, ShapeSupportData& support_data) { // TODO add benchmark to set a proper value for switching between linear and // logarithmic. @@ -435,44 +417,38 @@ void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, support_data); } } -// clang-format off -getShapeSupportTplInstantiation(ConvexBase) - // clang-format on - - // ============================================================================ - template - inline void getShapeSupport(const SmallConvex* convex, const Vec3f& dir, - Vec3f& support, int& hint, - ShapeSupportData& support_data) { +getShapeSupportTplInstantiation(ConvexBase); + +// ============================================================================ +template +inline void getShapeSupport(const SmallConvex* convex, const Vec3s& dir, + Vec3s& support, int& hint, + ShapeSupportData& support_data) { getShapeSupportLinear<_SupportOptions>( reinterpret_cast(convex), dir, support, hint, support_data); } -// clang-format off -getShapeSupportTplInstantiation(SmallConvex) - // clang-format on - - // ============================================================================ - template - inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, - Vec3f& support, int& hint, - ShapeSupportData& support_data) { +getShapeSupportTplInstantiation(SmallConvex); + +// ============================================================================ +template +inline void getShapeSupport(const LargeConvex* convex, const Vec3s& dir, + Vec3s& support, int& hint, + ShapeSupportData& support_data) { getShapeSupportLog<_SupportOptions>( reinterpret_cast(convex), dir, support, hint, support_data); } -// clang-format off -getShapeSupportTplInstantiation(LargeConvex) -// clang-format on +getShapeSupportTplInstantiation(LargeConvex); // ============================================================================ #define CALL_GET_SHAPE_SUPPORT_SET(ShapeType) \ getShapeSupportSet<_SupportOptions>(static_cast(shape), \ support_set, hint, support_data, \ max_num_supports, tol) - template - void getSupportSet(const ShapeBase* shape, SupportSet& support_set, - int& hint, size_t max_num_supports, FCL_REAL tol) { +template +void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint, + size_t max_num_supports, CoalScalar tol) { ShapeSupportData support_data; switch (shape->getNodeType()) { case GEOM_TRIANGLE: @@ -508,39 +484,38 @@ getShapeSupportTplInstantiation(LargeConvex) // Explicit instantiation // clang-format off -template HPP_FCL_DLLAPI void getSupportSet(const ShapeBase*, SupportSet&, int&, size_t, FCL_REAL); +template COAL_DLLAPI void getSupportSet(const ShapeBase*, SupportSet&, int&, size_t, CoalScalar); -template HPP_FCL_DLLAPI void getSupportSet(const ShapeBase*, SupportSet&, int&, size_t, FCL_REAL); +template COAL_DLLAPI void getSupportSet(const ShapeBase*, SupportSet&, int&, size_t, CoalScalar); // clang-format on // ============================================================================ -#define getShapeSupportSetTplInstantiation(ShapeType) \ - template HPP_FCL_DLLAPI void \ - getShapeSupportSet( \ - const ShapeType* shape_, SupportSet& support_set, int& hint, \ - ShapeSupportData& data, size_t num_sampled_supports, FCL_REAL tol); \ - \ - template HPP_FCL_DLLAPI void \ - getShapeSupportSet( \ - const ShapeType* shape_, SupportSet& support_set, int& hint, \ - ShapeSupportData& data, size_t num_sampled_supports, FCL_REAL tol); +#define getShapeSupportSetTplInstantiation(ShapeType) \ + template COAL_DLLAPI void getShapeSupportSet( \ + const ShapeType* shape_, SupportSet& support_set, int& hint, \ + ShapeSupportData& data, size_t num_sampled_supports, CoalScalar tol); \ + \ + template COAL_DLLAPI void \ + getShapeSupportSet( \ + const ShapeType* shape_, SupportSet& support_set, int& hint, \ + ShapeSupportData& data, size_t num_sampled_supports, CoalScalar tol); // ============================================================================ template void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, - size_t /*unused*/, FCL_REAL tol) { + size_t /*unused*/, CoalScalar tol) { assert(tol > 0); support_set.clear(); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); // We simply want to compute the support value, no need to take the // swept-sphere radius into account. getShapeSupport(triangle, support_dir, support, hint, support_data); - const FCL_REAL support_value = support.dot(support_dir); + const CoalScalar support_value = support.dot(support_dir); if (support_value - support_dir.dot(triangle->a) < tol) { // Note: at the moment, it's useless to take into account the @@ -570,115 +545,108 @@ void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set, } } } -// clang-format off -getShapeSupportSetTplInstantiation(TriangleP) +getShapeSupportSetTplInstantiation(TriangleP); // ============================================================================ template void getShapeSupportSet(const Box* box, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data, - size_t /*unused*/, FCL_REAL tol) { - // clang-format on + size_t /*unused*/, CoalScalar tol) { assert(tol > 0); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); getShapeSupport(box, support_dir, support, hint, support_data); - const FCL_REAL support_value = support.dot(support_dir); - - const FCL_REAL x = box->halfSide[0]; - const FCL_REAL y = box->halfSide[1]; - const FCL_REAL z = box->halfSide[2]; - const std::array corners = { - Vec3f(x, y, z), Vec3f(-x, y, z), Vec3f(-x, -y, z), Vec3f(x, -y, z), - Vec3f(x, y, -z), Vec3f(-x, y, -z), Vec3f(-x, -y, -z), Vec3f(x, -y, -z), + const CoalScalar support_value = support.dot(support_dir); + + const CoalScalar x = box->halfSide[0]; + const CoalScalar y = box->halfSide[1]; + const CoalScalar z = box->halfSide[2]; + const std::array corners = { + Vec3s(x, y, z), Vec3s(-x, y, z), Vec3s(-x, -y, z), Vec3s(x, -y, z), + Vec3s(x, y, -z), Vec3s(-x, y, -z), Vec3s(-x, -y, -z), Vec3s(x, -y, -z), }; SupportSet::Polygon& polygon = support_data.polygon; polygon.clear(); - const Transform3f& tf = support_set.tf; - for (const Vec3f& corner : corners) { - const FCL_REAL val = corner.dot(support_dir); + const Transform3s& tf = support_set.tf; + for (const Vec3s& corner : corners) { + const CoalScalar val = corner.dot(support_dir); if (support_value - val < tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { - const Vec2f p = + const Vec2s p = tf.inverseTransform(corner + box->getSweptSphereRadius() * support_dir) .template head<2>(); polygon.emplace_back(p); } else { - const Vec2f p = tf.inverseTransform(corner).template head<2>(); + const Vec2s p = tf.inverseTransform(corner).template head<2>(); polygon.emplace_back(p); } } } computeSupportSetConvexHull(polygon, support_set.points()); } -// clang-format off -getShapeSupportSetTplInstantiation(Box) +getShapeSupportSetTplInstantiation(Box); // ============================================================================ template void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, - size_t /*unused*/, FCL_REAL /*unused*/) { - // clang-format on + size_t /*unused*/, CoalScalar /*unused*/) { support_set.points().clear(); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); getShapeSupport<_SupportOptions>(sphere, support_dir, support, hint, support_data); support_set.addPoint(support); } -// clang-format off -getShapeSupportSetTplInstantiation(Sphere) +getShapeSupportSetTplInstantiation(Sphere); // ============================================================================ template void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set, int& hint, ShapeSupportData& support_data /*unused*/, - size_t /*unused*/, FCL_REAL /*unused*/) { - // clang-format on + size_t /*unused*/, CoalScalar /*unused*/) { support_set.points().clear(); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); getShapeSupport<_SupportOptions>(ellipsoid, support_dir, support, hint, support_data); support_set.addPoint(support); } -// clang-format off -getShapeSupportSetTplInstantiation(Ellipsoid) +getShapeSupportSetTplInstantiation(Ellipsoid); // ============================================================================ template void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, - size_t /*unused*/, FCL_REAL tol) { + size_t /*unused*/, CoalScalar tol) { // clang-format on assert(tol > 0); support_set.points().clear(); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); getShapeSupport(capsule, support_dir, support, hint, support_data); - const FCL_REAL support_value = + const CoalScalar support_value = support_dir.dot(support + capsule->radius * support_dir); // The support set of a capsule has either 2 points or 1 point. // The two candidate points lie at the frontier between the cylinder and // sphere parts of the capsule. - const FCL_REAL h = capsule->halfLength; - const FCL_REAL r = capsule->radius; - const Vec3f p1(r * support_dir[0], r * support_dir[1], h); - const Vec3f p2(r * support_dir[0], r * support_dir[1], -h); + const CoalScalar h = capsule->halfLength; + const CoalScalar r = capsule->radius; + const Vec3s p1(r * support_dir[0], r * support_dir[1], h); + const Vec3s p2(r * support_dir[0], r * support_dir[1], -h); if ((support_value - support_dir.dot(p1) <= tol) && (support_value - support_dir.dot(p2) <= tol)) { if (_SupportOptions == SupportOptions::WithSweptSphere) { - const Vec3f ssr_vec = support_dir * capsule->getSweptSphereRadius(); + const Vec3s ssr_vec = support_dir * capsule->getSweptSphereRadius(); support_set.addPoint(p1 + ssr_vec); support_set.addPoint(p2 + ssr_vec); } else { @@ -687,45 +655,43 @@ void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set, } } else { if (_SupportOptions == SupportOptions::WithSweptSphere) { - const Vec3f ssr_vec = support_dir * capsule->getSweptSphereRadius(); + const Vec3s ssr_vec = support_dir * capsule->getSweptSphereRadius(); support_set.addPoint(support + ssr_vec); } else { support_set.addPoint(support); } } } -// clang-format off -getShapeSupportSetTplInstantiation(Capsule) +getShapeSupportSetTplInstantiation(Capsule); // ============================================================================ template void getShapeSupportSet(const Cone* cone, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, - size_t num_sampled_supports, FCL_REAL tol) { - // clang-format on + size_t num_sampled_supports, CoalScalar tol) { assert(tol > 0); support_set.points().clear(); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); getShapeSupport(cone, support_dir, support, hint, support_data); - const FCL_REAL support_value = support.dot(support_dir); + const CoalScalar support_value = support.dot(support_dir); // If the support direction is perpendicular to the cone's basis, there is an // infinite amount of support points; otherwise there are up to two support // points (two if direction is perpendicular to the side of the cone and one // otherwise). // - // To check this condition, we look at two points on the cone's basis; these - // two points are symmetrical w.r.t the center of the circle. If both these - // points are tol away from the support plane, then all the points of the - // circle are tol away from the support plane. - const FCL_REAL r = cone->radius; - const FCL_REAL z = -cone->halfLength; - const Vec3f p1(r * support_dir[0], r * support_dir[1], z); - const Vec3f p2(-r * support_dir[0], -r * support_dir[1], z); + // To check this condition, we look at two points on the cone's basis; + // these two points are symmetrical w.r.t the center of the circle. If + // both these points are tol away from the support plane, then all the + // points of the circle are tol away from the support plane. + const CoalScalar r = cone->radius; + const CoalScalar z = -cone->halfLength; + const Vec3s p1(r * support_dir[0], r * support_dir[1], z); + const Vec3s p2(-r * support_dir[0], -r * support_dir[1], z); if ((support_value - support_dir.dot(p1) <= tol) && (support_value - support_dir.dot(p2) <= tol)) { @@ -733,11 +699,11 @@ void getShapeSupportSet(const Cone* cone, SupportSet& support_set, // the basis of the cone. We sample `num_sampled_supports` points on the // base of the cone. We are guaranteed that these points like at a distance // tol of the support plane. - const FCL_REAL angle_increment = - 2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(num_sampled_supports)); + const CoalScalar angle_increment = + 2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(num_sampled_supports)); for (size_t i = 0; i < num_sampled_supports; ++i) { - const FCL_REAL theta = (FCL_REAL)(i)*angle_increment; - Vec3f point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z); + const CoalScalar theta = (CoalScalar)(i)*angle_increment; + Vec3s point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z); assert(std::abs(support_dir.dot(support - point_on_cone_base)) <= tol); if (_SupportOptions == SupportOptions::WithSweptSphere) { point_on_cone_base += cone->getSweptSphereRadius() * support_dir; @@ -748,7 +714,7 @@ void getShapeSupportSet(const Cone* cone, SupportSet& support_set, // There are two potential supports to add: the tip of the cone and a point // on the basis of the cone. We compare each of these points to the support // value. - Vec3f cone_tip(0, 0, cone->halfLength); + Vec3s cone_tip(0, 0, cone->halfLength); if (support_value - support_dir.dot(cone_tip) <= tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { cone_tip += cone->getSweptSphereRadius() * support_dir; @@ -756,7 +722,7 @@ void getShapeSupportSet(const Cone* cone, SupportSet& support_set, support_set.addPoint(cone_tip); } - Vec3f point_on_cone_base = Vec3f(cone->radius * support_dir[0], // + Vec3s point_on_cone_base = Vec3s(cone->radius * support_dir[0], // cone->radius * support_dir[1], // z); if (support_value - support_dir.dot(point_on_cone_base) <= tol) { @@ -767,40 +733,38 @@ void getShapeSupportSet(const Cone* cone, SupportSet& support_set, } } } -// clang-format off -getShapeSupportSetTplInstantiation(Cone) +getShapeSupportSetTplInstantiation(Cone); // ============================================================================ template void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, - size_t num_sampled_supports, FCL_REAL tol) { - // clang-format on + size_t num_sampled_supports, CoalScalar tol) { assert(tol > 0); support_set.points().clear(); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); getShapeSupport(cylinder, support_dir, support, hint, support_data); - const FCL_REAL support_value = support.dot(support_dir); + const CoalScalar support_value = support.dot(support_dir); // The following is very similar to what is done for Cone's support set // computation. - const FCL_REAL r = cylinder->radius; - const FCL_REAL z = + const CoalScalar r = cylinder->radius; + const CoalScalar z = support_dir[2] <= 0 ? -cylinder->halfLength : cylinder->halfLength; - const Vec3f p1(r * support_dir[0], r * support_dir[1], z); - const Vec3f p2(-r * support_dir[0], -r * support_dir[1], z); + const Vec3s p1(r * support_dir[0], r * support_dir[1], z); + const Vec3s p2(-r * support_dir[0], -r * support_dir[1], z); if ((support_value - support_dir.dot(p1) <= tol) && (support_value - support_dir.dot(p2) <= tol)) { - const FCL_REAL angle_increment = - 2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(num_sampled_supports)); + const CoalScalar angle_increment = + 2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(num_sampled_supports)); for (size_t i = 0; i < num_sampled_supports; ++i) { - const FCL_REAL theta = (FCL_REAL)(i)*angle_increment; - Vec3f point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z); + const CoalScalar theta = (CoalScalar)(i)*angle_increment; + Vec3s point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z); assert(std::abs(support_dir.dot(support - point_on_cone_base)) <= tol); if (_SupportOptions == SupportOptions::WithSweptSphere) { point_on_cone_base += cylinder->getSweptSphereRadius() * support_dir; @@ -810,7 +774,7 @@ void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set, } else { // There are two potential supports to add: one on each circle bases of the // cylinder. - Vec3f point_on_lower_circle = Vec3f(cylinder->radius * support_dir[0], // + Vec3s point_on_lower_circle = Vec3s(cylinder->radius * support_dir[0], // cylinder->radius * support_dir[1], // -cylinder->halfLength); if (support_value - support_dir.dot(point_on_lower_circle) <= tol) { @@ -820,7 +784,7 @@ void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set, support_set.addPoint(point_on_lower_circle); } - Vec3f point_on_upper_circle = Vec3f(cylinder->radius * support_dir[0], // + Vec3s point_on_upper_circle = Vec3s(cylinder->radius * support_dir[0], // cylinder->radius * support_dir[1], // cylinder->halfLength); if (support_value - support_dir.dot(point_on_upper_circle) <= tol) { @@ -831,38 +795,36 @@ void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set, } } } -// clang-format off -getShapeSupportSetTplInstantiation(Cylinder) +getShapeSupportSetTplInstantiation(Cylinder); // ============================================================================ template void getShapeSupportSetLinear(const ConvexBase* convex, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data, size_t /*unused*/, - FCL_REAL tol) { - // clang-format on + CoalScalar tol) { assert(tol > 0); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); getShapeSupport(convex, support_dir, support, hint, support_data); - const FCL_REAL support_value = support_dir.dot(support); + const CoalScalar support_value = support_dir.dot(support); - const std::vector& points = *(convex->points); + const std::vector& points = *(convex->points); SupportSet::Polygon& polygon = support_data.polygon; polygon.clear(); - const Transform3f& tf = support_set.tf; - for (const Vec3f& point : points) { - const FCL_REAL dot = support_dir.dot(point); + const Transform3s& tf = support_set.tf; + for (const Vec3s& point : points) { + const CoalScalar dot = support_dir.dot(point); if (support_value - dot <= tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { - const Vec2f p = + const Vec2s p = tf.inverseTransform(point + convex->getSweptSphereRadius() * support_dir) .template head<2>(); polygon.emplace_back(p); } else { - const Vec2f p = tf.inverseTransform(point).template head<2>(); + const Vec2s p = tf.inverseTransform(point).template head<2>(); polygon.emplace_back(p); } } @@ -874,30 +836,30 @@ void getShapeSupportSetLinear(const ConvexBase* convex, SupportSet& support_set, // ============================================================================ template void convexSupportSetRecurse( - const std::vector& points, + const std::vector& points, const std::vector& neighbors, - const FCL_REAL swept_sphere_radius, const size_t vertex_idx, - const Vec3f& support_dir, const FCL_REAL support_value, - const Transform3f& tf, std::vector& visited, - SupportSet::Polygon& polygon, FCL_REAL tol) { - HPP_FCL_UNUSED_VARIABLE(swept_sphere_radius); + const CoalScalar swept_sphere_radius, const size_t vertex_idx, + const Vec3s& support_dir, const CoalScalar support_value, + const Transform3s& tf, std::vector& visited, + SupportSet::Polygon& polygon, CoalScalar tol) { + COAL_UNUSED_VARIABLE(swept_sphere_radius); if (visited[vertex_idx]) { return; } visited[vertex_idx] = true; - const Vec3f& point = points[vertex_idx]; - const FCL_REAL val = point.dot(support_dir); + const Vec3s& point = points[vertex_idx]; + const CoalScalar val = point.dot(support_dir); if (support_value - val <= tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { - const Vec2f p = + const Vec2s p = tf.inverseTransform(point + swept_sphere_radius * support_dir) .template head<2>(); polygon.emplace_back(p); } else { - const Vec2f p = tf.inverseTransform(point).template head<2>(); + const Vec2s p = tf.inverseTransform(point).template head<2>(); polygon.emplace_back(p); } @@ -915,17 +877,17 @@ void convexSupportSetRecurse( template void getShapeSupportSetLog(const ConvexBase* convex, SupportSet& support_set, int& hint, ShapeSupportData& support_data, - size_t /*unused*/, FCL_REAL tol) { + size_t /*unused*/, CoalScalar tol) { assert(tol > 0); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); getShapeSupportLog( convex, support_dir, support, hint, support_data); - const FCL_REAL support_value = support.dot(support_dir); + const CoalScalar support_value = support.dot(support_dir); - const std::vector& points = *(convex->points); + const std::vector& points = *(convex->points); const std::vector& neighbors = *(convex->neighbors); - const FCL_REAL swept_sphere_radius = convex->getSweptSphereRadius(); + const CoalScalar swept_sphere_radius = convex->getSweptSphereRadius(); std::vector& visited = support_data.visited; // `visited` is guaranteed to be of right size due to previous call to convex // log support function. @@ -933,7 +895,7 @@ void getShapeSupportSetLog(const ConvexBase* convex, SupportSet& support_set, SupportSet::Polygon& polygon = support_data.polygon; polygon.clear(); - const Transform3f& tf = support_set.tf; + const Transform3s& tf = support_set.tf; const size_t vertex_idx = (size_t)(hint); convexSupportSetRecurse<_SupportOptions>( @@ -947,7 +909,8 @@ void getShapeSupportSetLog(const ConvexBase* convex, SupportSet& support_set, template void getShapeSupportSet(const ConvexBase* convex, SupportSet& support_set, int& hint, ShapeSupportData& support_data, - size_t num_sampled_supports /*unused*/, FCL_REAL tol) { + size_t num_sampled_supports /*unused*/, + CoalScalar tol) { if (convex->num_points > ConvexBase::num_vertices_large_convex_threshold && convex->neighbors != nullptr) { getShapeSupportSetLog<_SupportOptions>( @@ -957,46 +920,42 @@ void getShapeSupportSet(const ConvexBase* convex, SupportSet& support_set, convex, support_set, hint, support_data, num_sampled_supports, tol); } } -// clang-format off -getShapeSupportSetTplInstantiation(ConvexBase) +getShapeSupportSetTplInstantiation(ConvexBase); // ============================================================================ template void getShapeSupportSet(const SmallConvex* convex, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, - size_t num_sampled_supports /*unused*/, FCL_REAL tol) { - // clang-format on + size_t num_sampled_supports /*unused*/, + CoalScalar tol) { getShapeSupportSetLinear<_SupportOptions>( reinterpret_cast(convex), support_set, hint, support_data, num_sampled_supports, tol); } -// clang-format off -getShapeSupportSetTplInstantiation(SmallConvex) +getShapeSupportSetTplInstantiation(SmallConvex); // ============================================================================ template void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set, int& hint, ShapeSupportData& support_data, - size_t num_sampled_supports /*unused*/, FCL_REAL tol) { - // clang-format on + size_t num_sampled_supports /*unused*/, + CoalScalar tol) { getShapeSupportSetLog<_SupportOptions>( reinterpret_cast(convex), support_set, hint, support_data, num_sampled_supports, tol); } -//clang-format off -getShapeSupportSetTplInstantiation(LargeConvex) - // clang-format on - - // ============================================================================ - HPP_FCL_DLLAPI - void computeSupportSetConvexHull(SupportSet::Polygon& cloud, - SupportSet::Polygon& cvx_hull) { +getShapeSupportSetTplInstantiation(LargeConvex); + +// ============================================================================ +COAL_DLLAPI +void computeSupportSetConvexHull(SupportSet::Polygon& cloud, + SupportSet::Polygon& cvx_hull) { cvx_hull.clear(); if (cloud.size() <= 2) { // Point or segment, nothing to do. - for (const Vec2f& point : cloud) { + for (const Vec2s& point : cloud) { cvx_hull.emplace_back(point); } return; @@ -1013,16 +972,16 @@ getShapeSupportSetTplInstantiation(LargeConvex) if (cloud[0](1) > cloud[2](1)) { std::swap(cloud[0], cloud[2]); } - const Vec2f& a = cloud[0]; - const Vec2f& b = cloud[1]; - const Vec2f& c = cloud[2]; - const FCL_REAL det = + const Vec2s& a = cloud[0]; + const Vec2s& b = cloud[1]; + const Vec2s& c = cloud[2]; + const CoalScalar det = (b(0) - a(0)) * (c(1) - a(1)) - (b(1) - a(1)) * (c(0) - a(0)); if (det < 0) { std::swap(cloud[1], cloud[2]); } - for (const Vec2f& point : cloud) { + for (const Vec2s& point : cloud) { cvx_hull.emplace_back(point); } return; @@ -1036,9 +995,9 @@ getShapeSupportSetTplInstantiation(LargeConvex) // in the direction (0, -1) (take the element of the set which has the lowest // y coordinate). size_t support_idx = 0; - FCL_REAL support_val = cloud[0](1); + CoalScalar support_val = cloud[0](1); for (size_t i = 1; i < cloud.size(); ++i) { - const FCL_REAL val = cloud[i](1); + const CoalScalar val = cloud[i](1); if (val < support_val) { support_val = val; support_idx = i; @@ -1047,17 +1006,17 @@ getShapeSupportSetTplInstantiation(LargeConvex) std::swap(cloud[0], cloud[support_idx]); cvx_hull.clear(); cvx_hull.emplace_back(cloud[0]); - const Vec2f& v = cvx_hull[0]; + const Vec2s& v = cvx_hull[0]; // Step 2 - Sort the rest of the point cloud according to the angle made with // v. Note: we use stable_sort instead of sort because sort can fail if two // values are identical. std::stable_sort( - cloud.begin() + 1, cloud.end(), [&v](const Vec2f& p1, const Vec2f& p2) { + cloud.begin() + 1, cloud.end(), [&v](const Vec2s& p1, const Vec2s& p2) { // p1 is "smaller" than p2 if det(p1 - v, p2 - v) >= 0 - const FCL_REAL det = + const CoalScalar det = (p1(0) - v(0)) * (p2(1) - v(1)) - (p1(1) - v(1)) * (p2(0) - v(0)); - if (std::abs(det) <= Eigen::NumTraits::dummy_precision()) { + if (std::abs(det) <= Eigen::NumTraits::dummy_precision()) { // If two points are identical or (v, p1, p2) are colinear, p1 is // "smaller" if it is closer to v. return ((p1 - v).squaredNorm() <= (p2 - v).squaredNorm()); @@ -1069,14 +1028,14 @@ getShapeSupportSetTplInstantiation(LargeConvex) // to the cvx-hull if they successively form "left turns" only. A left turn // is: considering the last three points of the cvx-hull, if they form a // right-hand basis (determinant > 0) then they make a left turn. - auto isRightSided = [](const Vec2f& p1, const Vec2f& p2, const Vec2f& p3) { + auto isRightSided = [](const Vec2s& p1, const Vec2s& p2, const Vec2s& p3) { // Checks if (p2 - p1, p3 - p1) forms a right-sided base based on // det(p2 - p1, p3 - p1) - const FCL_REAL det = + const CoalScalar det = (p2(0) - p1(0)) * (p3(1) - p1(1)) - (p2(1) - p1(1)) * (p3(0) - p1(0)); // Note: we set a dummy precision threshold so that identical points or // colinear pionts are not added to the cvx-hull. - return det > Eigen::NumTraits::dummy_precision(); + return det > Eigen::NumTraits::dummy_precision(); }; // We initialize the cvx-hull algo by adding the first three @@ -1085,9 +1044,9 @@ getShapeSupportSetTplInstantiation(LargeConvex) // to form a right sided basis, hence to form a left turn. size_t cloud_beginning_idx = 1; while (cvx_hull.size() < 3) { - const Vec2f& vec = cloud[cloud_beginning_idx]; + const Vec2s& vec = cloud[cloud_beginning_idx]; if ((cvx_hull.back() - vec).squaredNorm() > - Eigen::NumTraits::epsilon()) { + Eigen::NumTraits::epsilon()) { cvx_hull.emplace_back(vec); } ++cloud_beginning_idx; @@ -1098,7 +1057,7 @@ getShapeSupportSetTplInstantiation(LargeConvex) // When we do a turn in the correct direction, we add a point to the // convex-hull. for (size_t i = cloud_beginning_idx; i < cloud.size(); ++i) { - const Vec2f& vec = cloud[i]; + const Vec2s& vec = cloud[i]; while (cvx_hull.size() > 1 && !isRightSided(cvx_hull[cvx_hull.size() - 2], cvx_hull[cvx_hull.size() - 1], vec)) { @@ -1109,5 +1068,4 @@ getShapeSupportSetTplInstantiation(LargeConvex) } } // namespace details -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/octree.cpp b/src/octree.cpp index 7e0160596..186ad51a9 100644 --- a/src/octree.cpp +++ b/src/octree.cpp @@ -32,11 +32,11 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include "coal/octree.h" + #include -namespace hpp { -namespace fcl { +namespace coal { namespace internal { struct Neighbors { char value; @@ -55,29 +55,29 @@ struct Neighbors { void hasNeighboordPlusZ() { value |= 0x20; } }; // struct neighbors -void computeNeighbors(const std::vector& boxes, +void computeNeighbors(const std::vector& boxes, std::vector& neighbors) { - typedef std::vector VectorVec6f; - FCL_REAL fixedSize = -1; - FCL_REAL e(1e-8); + typedef std::vector VectorVec6s; + CoalScalar fixedSize = -1; + CoalScalar e(1e-8); for (std::size_t i = 0; i < boxes.size(); ++i) { - const Vec6f& box(boxes[i]); + const Vec6s& box(boxes[i]); Neighbors& n(neighbors[i]); - FCL_REAL x(box[0]); - FCL_REAL y(box[1]); - FCL_REAL z(box[2]); - FCL_REAL s(box[3]); + CoalScalar x(box[0]); + CoalScalar y(box[1]); + CoalScalar z(box[2]); + CoalScalar s(box[3]); if (fixedSize == -1) fixedSize = s; else assert(s == fixedSize); - for (VectorVec6f::const_iterator it = boxes.begin(); it != boxes.end(); + for (VectorVec6s::const_iterator it = boxes.begin(); it != boxes.end(); ++it) { - const Vec6f& otherBox = *it; - FCL_REAL xo(otherBox[0]); - FCL_REAL yo(otherBox[1]); - FCL_REAL zo(otherBox[2]); + const Vec6s& otherBox = *it; + CoalScalar xo(otherBox[0]); + CoalScalar yo(otherBox[1]); + CoalScalar zo(otherBox[2]); // if (fabs(x-xo) < e && fabs(y-yo) < e && fabs(z-zo) < e){ // continue; // } @@ -106,35 +106,35 @@ void computeNeighbors(const std::vector& boxes, } // namespace internal void OcTree::exportAsObjFile(const std::string& filename) const { - std::vector boxes(this->toBoxes()); + std::vector boxes(this->toBoxes()); std::vector neighbors(boxes.size()); internal::computeNeighbors(boxes, neighbors); // compute list of vertices and faces - typedef std::vector VectorVec3f; - std::vector vertices; + typedef std::vector VectorVec3s; + std::vector vertices; typedef std::array Array4; typedef std::vector VectorArray4; std::vector faces; for (std::size_t i = 0; i < boxes.size(); ++i) { - const Vec6f& box(boxes[i]); + const Vec6s& box(boxes[i]); internal::Neighbors& n(neighbors[i]); - FCL_REAL x(box[0]); - FCL_REAL y(box[1]); - FCL_REAL z(box[2]); - FCL_REAL size(box[3]); + CoalScalar x(box[0]); + CoalScalar y(box[1]); + CoalScalar z(box[2]); + CoalScalar size(box[3]); - vertices.push_back(Vec3f(x - .5 * size, y - .5 * size, z - .5 * size)); - vertices.push_back(Vec3f(x + .5 * size, y - .5 * size, z - .5 * size)); - vertices.push_back(Vec3f(x - .5 * size, y + .5 * size, z - .5 * size)); - vertices.push_back(Vec3f(x + .5 * size, y + .5 * size, z - .5 * size)); - vertices.push_back(Vec3f(x - .5 * size, y - .5 * size, z + .5 * size)); - vertices.push_back(Vec3f(x + .5 * size, y - .5 * size, z + .5 * size)); - vertices.push_back(Vec3f(x - .5 * size, y + .5 * size, z + .5 * size)); - vertices.push_back(Vec3f(x + .5 * size, y + .5 * size, z + .5 * size)); + vertices.push_back(Vec3s(x - .5 * size, y - .5 * size, z - .5 * size)); + vertices.push_back(Vec3s(x + .5 * size, y - .5 * size, z - .5 * size)); + vertices.push_back(Vec3s(x - .5 * size, y + .5 * size, z - .5 * size)); + vertices.push_back(Vec3s(x + .5 * size, y + .5 * size, z - .5 * size)); + vertices.push_back(Vec3s(x - .5 * size, y - .5 * size, z + .5 * size)); + vertices.push_back(Vec3s(x + .5 * size, y - .5 * size, z + .5 * size)); + vertices.push_back(Vec3s(x - .5 * size, y + .5 * size, z + .5 * size)); + vertices.push_back(Vec3s(x + .5 * size, y + .5 * size, z + .5 * size)); // Add face only if box has no neighbor with the same face if (!n.minusX()) { @@ -166,15 +166,15 @@ void OcTree::exportAsObjFile(const std::string& filename) const { std::ofstream os; os.open(filename); if (!os.is_open()) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( (std::string("failed to open file \"") + filename + std::string("\"")) .c_str(), std::runtime_error); // write vertices os << "# list of vertices\n"; - for (VectorVec3f::const_iterator it = vertices.begin(); it != vertices.end(); + for (VectorVec3s::const_iterator it = vertices.begin(); it != vertices.end(); ++it) { - const Vec3f& v = *it; + const Vec3s& v = *it; os << "v " << v[0] << " " << v[1] << " " << v[2] << '\n'; } os << "\n# list of faces\n"; @@ -186,9 +186,9 @@ void OcTree::exportAsObjFile(const std::string& filename) const { } OcTreePtr_t makeOctree( - const Eigen::Matrix& point_cloud, - const FCL_REAL resolution) { - typedef Eigen::Matrix InputType; + const Eigen::Matrix& point_cloud, + const CoalScalar resolution) { + typedef Eigen::Matrix InputType; typedef InputType::ConstRowXpr RowType; shared_ptr octree(new octomap::OcTree(resolution)); @@ -200,5 +200,4 @@ OcTreePtr_t makeOctree( return OcTreePtr_t(new OcTree(octree)); } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/serialization/serialization.cpp b/src/serialization/serialization.cpp index af6763b31..692e79ddf 100644 --- a/src/serialization/serialization.cpp +++ b/src/serialization/serialization.cpp @@ -34,51 +34,51 @@ /** \author Justin Carpentier */ -#include "hpp/fcl/serialization/fwd.h" +#include "coal/serialization/fwd.h" -using namespace hpp::fcl; +using namespace coal; -#include "hpp/fcl/serialization/transform.h" -#include "hpp/fcl/serialization/collision_data.h" -#include "hpp/fcl/serialization/geometric_shapes.h" -#include "hpp/fcl/serialization/convex.h" -#include "hpp/fcl/serialization/hfield.h" -#include "hpp/fcl/serialization/BVH_model.h" -#ifdef HPP_FCL_HAS_OCTOMAP -#include "hpp/fcl/serialization/octree.h" +#include "coal/serialization/transform.h" +#include "coal/serialization/collision_data.h" +#include "coal/serialization/geometric_shapes.h" +#include "coal/serialization/convex.h" +#include "coal/serialization/hfield.h" +#include "coal/serialization/BVH_model.h" +#ifdef COAL_HAS_OCTOMAP +#include "coal/serialization/octree.h" #endif -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(CollisionRequest) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(CollisionResult) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(DistanceRequest) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(DistanceResult) +COAL_SERIALIZATION_DEFINE_EXPORT(CollisionRequest) +COAL_SERIALIZATION_DEFINE_EXPORT(CollisionResult) +COAL_SERIALIZATION_DEFINE_EXPORT(DistanceRequest) +COAL_SERIALIZATION_DEFINE_EXPORT(DistanceResult) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(ShapeBase) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(CollisionGeometry) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(TriangleP) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Box) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Sphere) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Ellipsoid) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Capsule) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Cone) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Cylinder) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Halfspace) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Plane) +COAL_SERIALIZATION_DEFINE_EXPORT(ShapeBase) +COAL_SERIALIZATION_DEFINE_EXPORT(CollisionGeometry) +COAL_SERIALIZATION_DEFINE_EXPORT(TriangleP) +COAL_SERIALIZATION_DEFINE_EXPORT(Box) +COAL_SERIALIZATION_DEFINE_EXPORT(Sphere) +COAL_SERIALIZATION_DEFINE_EXPORT(Ellipsoid) +COAL_SERIALIZATION_DEFINE_EXPORT(Capsule) +COAL_SERIALIZATION_DEFINE_EXPORT(Cone) +COAL_SERIALIZATION_DEFINE_EXPORT(Cylinder) +COAL_SERIALIZATION_DEFINE_EXPORT(Halfspace) +COAL_SERIALIZATION_DEFINE_EXPORT(Plane) -#define EXPORT_AND_CAST(Derived, Base) \ - HPP_FCL_SERIALIZATION_CAST_REGISTER(Derived, Base) \ - HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Derived) \ +#define EXPORT_AND_CAST(Derived, Base) \ + COAL_SERIALIZATION_CAST_REGISTER(Derived, Base) \ + COAL_SERIALIZATION_DEFINE_EXPORT(Derived) \ /**/ -HPP_FCL_SERIALIZATION_CAST_REGISTER(ConvexBase, CollisionGeometry) +COAL_SERIALIZATION_CAST_REGISTER(ConvexBase, CollisionGeometry) EXPORT_AND_CAST(Convex, ConvexBase) EXPORT_AND_CAST(Convex, ConvexBase) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(HeightField) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(HeightField) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(HeightField) +COAL_SERIALIZATION_DEFINE_EXPORT(HeightField) +COAL_SERIALIZATION_DEFINE_EXPORT(HeightField) +COAL_SERIALIZATION_DEFINE_EXPORT(HeightField) -HPP_FCL_SERIALIZATION_CAST_REGISTER(BVHModelBase, CollisionGeometry) +COAL_SERIALIZATION_CAST_REGISTER(BVHModelBase, CollisionGeometry) EXPORT_AND_CAST(BVHModel, BVHModelBase) EXPORT_AND_CAST(BVHModel, BVHModelBase) @@ -89,6 +89,6 @@ EXPORT_AND_CAST(BVHModel>, BVHModelBase) EXPORT_AND_CAST(BVHModel>, BVHModelBase) EXPORT_AND_CAST(BVHModel>, BVHModelBase) -#ifdef HPP_FCL_HAS_OCTOMAP -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(OcTree) +#ifdef COAL_HAS_OCTOMAP +COAL_SERIALIZATION_DEFINE_EXPORT(OcTree) #endif diff --git a/src/shape/convex.cpp b/src/shape/convex.cpp index 74fb1d6aa..e32677a03 100644 --- a/src/shape/convex.cpp +++ b/src/shape/convex.cpp @@ -1,6 +1,6 @@ -#include +#include "coal/shape/convex.h" -#ifdef HPP_FCL_HAS_QHULL +#ifdef COAL_HAS_QHULL #include #include #include @@ -17,22 +17,21 @@ using orgQhull::QhullVertexList; using orgQhull::QhullVertexSet; #endif -namespace hpp { -namespace fcl { +namespace coal { // Reorders `tri` such that the dot product between the normal of triangle and // the vector `triangle barycentre - convex_tri.center` is positive. void reorderTriangle(const Convex* convex_tri, Triangle& tri) { - Vec3f p0, p1, p2; + Vec3s p0, p1, p2; p0 = (*(convex_tri->points))[tri[0]]; p1 = (*(convex_tri->points))[tri[1]]; p2 = (*(convex_tri->points))[tri[2]]; - Vec3f barycentre_tri, center_barycenter; + Vec3s barycentre_tri, center_barycenter; barycentre_tri = (p0 + p1 + p2) / 3; center_barycenter = barycentre_tri - convex_tri->center; - Vec3f edge_tri1, edge_tri2, n_tri; + Vec3s edge_tri1, edge_tri2, n_tri; edge_tri1 = p1 - p0; edge_tri2 = p2 - p1; n_tri = edge_tri1.cross(edge_tri2); @@ -42,22 +41,22 @@ void reorderTriangle(const Convex* convex_tri, Triangle& tri) { } } -ConvexBase* ConvexBase::convexHull(std::shared_ptr>& pts, +ConvexBase* ConvexBase::convexHull(std::shared_ptr>& pts, unsigned int num_points, bool keepTriangles, const char* qhullCommand) { - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS return ConvexBase::convexHull(pts->data(), num_points, keepTriangles, qhullCommand); - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP } -ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, +ConvexBase* ConvexBase::convexHull(const Vec3s* pts, unsigned int num_points, bool keepTriangles, const char* qhullCommand) { -#ifdef HPP_FCL_HAS_QHULL +#ifdef COAL_HAS_QHULL if (num_points <= 3) { - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "You shouldn't use this function with less than" " 4 points.", std::invalid_argument); @@ -71,7 +70,7 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, if (qh.qhullStatus() != qh_ERRnone) { if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl; - HPP_FCL_THROW_PRETTY("Qhull failed", std::logic_error); + COAL_THROW_PRETTY("Qhull failed", std::logic_error); } typedef std::size_t index_type; @@ -82,15 +81,15 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, // Initialize the vertices size_t nvertex = static_cast(qh.vertexCount()); - std::shared_ptr> vertices( - new std::vector(size_t(nvertex))); + std::shared_ptr> vertices( + new std::vector(size_t(nvertex))); QhullVertexList vertexList(qh.vertexList()); size_t i_vertex = 0; for (QhullVertexList::const_iterator v = vertexList.begin(); v != vertexList.end(); ++v) { QhullPoint pt((*v).point()); pts_to_vertices[(size_t)pt.id()] = (int)i_vertex; - (*vertices)[i_vertex] = Vec3f(pt[0], pt[1], pt[2]); + (*vertices)[i_vertex] = Vec3s(pt[0], pt[1], pt[2]); ++i_vertex; } assert(i_vertex == nvertex); @@ -144,7 +143,7 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, } } else { if (keepTriangles) { // TODO I think there is a memory leak here. - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "You requested to keep triangles so you " "must pass option \"Qt\" to qhull via the qhull command argument.", std::invalid_argument); @@ -184,7 +183,7 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, for (size_t i = 0; i < static_cast(nvertex); ++i) { Neighbors& n = neighbors_[i]; if (nneighbors[i].size() >= (std::numeric_limits::max)()) - HPP_FCL_THROW_PRETTY("Too many neighbors.", std::logic_error); + COAL_THROW_PRETTY("Too many neighbors.", std::logic_error); n.count_ = (unsigned char)nneighbors[i].size(); n.n_ = p_nneighbors; p_nneighbors = @@ -197,20 +196,20 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, convex->buildSupportWarmStart(); return convex; #else - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "Library built without qhull. Cannot build object of this type.", std::logic_error); - HPP_FCL_UNUSED_VARIABLE(pts); - HPP_FCL_UNUSED_VARIABLE(num_points); - HPP_FCL_UNUSED_VARIABLE(keepTriangles); - HPP_FCL_UNUSED_VARIABLE(qhullCommand); + COAL_UNUSED_VARIABLE(pts); + COAL_UNUSED_VARIABLE(num_points); + COAL_UNUSED_VARIABLE(keepTriangles); + COAL_UNUSED_VARIABLE(qhullCommand); #endif } -#ifdef HPP_FCL_HAS_QHULL +#ifdef COAL_HAS_QHULL void ConvexBase::buildDoubleDescription() { if (num_points <= 3) { - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "You shouldn't use this function with a convex less than" " 4 points.", std::invalid_argument); @@ -223,7 +222,7 @@ void ConvexBase::buildDoubleDescription() { if (qh.qhullStatus() != qh_ERRnone) { if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl; - HPP_FCL_THROW_PRETTY("Qhull failed", std::logic_error); + COAL_THROW_PRETTY("Qhull failed", std::logic_error); } buildDoubleDescriptionFromQHullResult(qh); @@ -231,15 +230,15 @@ void ConvexBase::buildDoubleDescription() { void ConvexBase::buildDoubleDescriptionFromQHullResult(const Qhull& qh) { num_normals_and_offsets = static_cast(qh.facetCount()); - normals.reset(new std::vector(num_normals_and_offsets)); - std::vector& normals_ = *normals; + normals.reset(new std::vector(num_normals_and_offsets)); + std::vector& normals_ = *normals; offsets.reset(new std::vector(num_normals_and_offsets)); std::vector& offsets_ = *offsets; unsigned int i_normal = 0; for (QhullFacet facet = qh.beginFacet(); facet != qh.endFacet(); facet = facet.next()) { const orgQhull::QhullHyperplane& plane = facet.hyperplane(); - normals_[i_normal] = Vec3f(plane.coordinates()[0], plane.coordinates()[1], + normals_[i_normal] = Vec3s(plane.coordinates()[0], plane.coordinates()[1], plane.coordinates()[2]); offsets_[i_normal] = plane.offset(); i_normal++; @@ -248,5 +247,4 @@ void ConvexBase::buildDoubleDescriptionFromQHullResult(const Qhull& qh) { } #endif -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp index 4c8af2a1f..cea609b3f 100644 --- a/src/shape/geometric_shapes.cpp +++ b/src/shape/geometric_shapes.cpp @@ -35,27 +35,26 @@ /** \author Jia Pan */ -#include -#include +#include "coal/shape/geometric_shapes.h" +#include "coal/shape/geometric_shapes_utility.h" -namespace hpp { -namespace fcl { +namespace coal { -void ConvexBase::initialize(std::shared_ptr> points_, +void ConvexBase::initialize(std::shared_ptr> points_, unsigned int num_points_) { this->points = points_; this->num_points = num_points_; - HPP_FCL_ASSERT(this->points->size() == this->num_points, - "The number of points is not consistent with the size of the " - "points vector", - std::logic_error); + COAL_ASSERT(this->points->size() == this->num_points, + "The number of points is not consistent with the size of the " + "points vector", + std::logic_error); this->num_normals_and_offsets = 0; this->normals.reset(); this->offsets.reset(); this->computeCenter(); } -void ConvexBase::set(std::shared_ptr> points_, +void ConvexBase::set(std::shared_ptr> points_, unsigned int num_points_) { initialize(points_, num_points_); } @@ -67,7 +66,7 @@ ConvexBase::ConvexBase(const ConvexBase& other) center(other.center) { if (other.points.get() && other.points->size() > 0) { // Deep copy of other points - points.reset(new std::vector(*other.points)); + points.reset(new std::vector(*other.points)); } else points.reset(); @@ -94,7 +93,7 @@ ConvexBase::ConvexBase(const ConvexBase& other) nneighbors_.reset(); if (other.normals.get() && other.normals->size() > 0) { - normals.reset(new std::vector(*(other.normals))); + normals.reset(new std::vector(*(other.normals))); } else normals.reset(); @@ -112,16 +111,16 @@ ConvexBase::~ConvexBase() {} void ConvexBase::computeCenter() { center.setZero(); - const std::vector& points_ = *points; + const std::vector& points_ = *points; for (std::size_t i = 0; i < num_points; ++i) center += points_[i]; // TODO(jcarpent): vectorization center /= num_points; } void Halfspace::unitNormalTest() { - FCL_REAL l = n.norm(); + CoalScalar l = n.norm(); if (l > 0) { - FCL_REAL inv_l = 1.0 / l; + CoalScalar inv_l = 1.0 / l; n *= inv_l; d *= inv_l; } else { @@ -131,9 +130,9 @@ void Halfspace::unitNormalTest() { } void Plane::unitNormalTest() { - FCL_REAL l = n.norm(); + CoalScalar l = n.norm(); if (l > 0) { - FCL_REAL inv_l = 1.0 / l; + CoalScalar inv_l = 1.0 / l; n *= inv_l; d *= inv_l; } else { @@ -143,115 +142,113 @@ void Plane::unitNormalTest() { } void Box::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + computeBV(*this, Transform3s(), aabb_local); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } void Sphere::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + computeBV(*this, Transform3s(), aabb_local); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = radius; } void Ellipsoid::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + computeBV(*this, Transform3s(), aabb_local); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } void Capsule::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + computeBV(*this, Transform3s(), aabb_local); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } void Cone::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + computeBV(*this, Transform3s(), aabb_local); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } void Cylinder::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + computeBV(*this, Transform3s(), aabb_local); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } void ConvexBase::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + computeBV(*this, Transform3s(), aabb_local); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } void Halfspace::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + computeBV(*this, Transform3s(), aabb_local); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } void Plane::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + computeBV(*this, Transform3s(), aabb_local); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } void TriangleP::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + computeBV(*this, Transform3s(), aabb_local); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp index d8cd7fc08..a687a8e30 100644 --- a/src/shape/geometric_shapes_utility.cpp +++ b/src/shape/geometric_shapes_utility.cpp @@ -35,198 +35,197 @@ /** \author Jia Pan */ -#include -#include -#include +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/internal/BV_fitter.h" +#include "coal/internal/tools.h" -namespace hpp { -namespace fcl { +namespace coal { namespace details { -std::vector getBoundVertices(const Box& box, const Transform3f& tf) { - std::vector result(8); - FCL_REAL a = box.halfSide[0]; - FCL_REAL b = box.halfSide[1]; - FCL_REAL c = box.halfSide[2]; - result[0] = tf.transform(Vec3f(a, b, c)); - result[1] = tf.transform(Vec3f(a, b, -c)); - result[2] = tf.transform(Vec3f(a, -b, c)); - result[3] = tf.transform(Vec3f(a, -b, -c)); - result[4] = tf.transform(Vec3f(-a, b, c)); - result[5] = tf.transform(Vec3f(-a, b, -c)); - result[6] = tf.transform(Vec3f(-a, -b, c)); - result[7] = tf.transform(Vec3f(-a, -b, -c)); +std::vector getBoundVertices(const Box& box, const Transform3s& tf) { + std::vector result(8); + CoalScalar a = box.halfSide[0]; + CoalScalar b = box.halfSide[1]; + CoalScalar c = box.halfSide[2]; + result[0] = tf.transform(Vec3s(a, b, c)); + result[1] = tf.transform(Vec3s(a, b, -c)); + result[2] = tf.transform(Vec3s(a, -b, c)); + result[3] = tf.transform(Vec3s(a, -b, -c)); + result[4] = tf.transform(Vec3s(-a, b, c)); + result[5] = tf.transform(Vec3s(-a, b, -c)); + result[6] = tf.transform(Vec3s(-a, -b, c)); + result[7] = tf.transform(Vec3s(-a, -b, -c)); return result; } // we use icosahedron to bound the sphere -std::vector getBoundVertices(const Sphere& sphere, - const Transform3f& tf) { - std::vector result(12); - const FCL_REAL m = (1 + sqrt(5.0)) / 2.0; - FCL_REAL edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0)); - - FCL_REAL a = edge_size; - FCL_REAL b = m * edge_size; - result[0] = tf.transform(Vec3f(0, a, b)); - result[1] = tf.transform(Vec3f(0, -a, b)); - result[2] = tf.transform(Vec3f(0, a, -b)); - result[3] = tf.transform(Vec3f(0, -a, -b)); - result[4] = tf.transform(Vec3f(a, b, 0)); - result[5] = tf.transform(Vec3f(-a, b, 0)); - result[6] = tf.transform(Vec3f(a, -b, 0)); - result[7] = tf.transform(Vec3f(-a, -b, 0)); - result[8] = tf.transform(Vec3f(b, 0, a)); - result[9] = tf.transform(Vec3f(b, 0, -a)); - result[10] = tf.transform(Vec3f(-b, 0, a)); - result[11] = tf.transform(Vec3f(-b, 0, -a)); +std::vector getBoundVertices(const Sphere& sphere, + const Transform3s& tf) { + std::vector result(12); + const CoalScalar m = (1 + sqrt(5.0)) / 2.0; + CoalScalar edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0)); + + CoalScalar a = edge_size; + CoalScalar b = m * edge_size; + result[0] = tf.transform(Vec3s(0, a, b)); + result[1] = tf.transform(Vec3s(0, -a, b)); + result[2] = tf.transform(Vec3s(0, a, -b)); + result[3] = tf.transform(Vec3s(0, -a, -b)); + result[4] = tf.transform(Vec3s(a, b, 0)); + result[5] = tf.transform(Vec3s(-a, b, 0)); + result[6] = tf.transform(Vec3s(a, -b, 0)); + result[7] = tf.transform(Vec3s(-a, -b, 0)); + result[8] = tf.transform(Vec3s(b, 0, a)); + result[9] = tf.transform(Vec3s(b, 0, -a)); + result[10] = tf.transform(Vec3s(-b, 0, a)); + result[11] = tf.transform(Vec3s(-b, 0, -a)); return result; } // we use scaled icosahedron to bound the ellipsoid -std::vector getBoundVertices(const Ellipsoid& ellipsoid, - const Transform3f& tf) { - std::vector result(12); - const FCL_REAL phi = (1 + sqrt(5.0)) / 2.0; - - const FCL_REAL a = sqrt(3.0) / (phi * phi); - const FCL_REAL b = phi * a; - - const FCL_REAL& A = ellipsoid.radii[0]; - const FCL_REAL& B = ellipsoid.radii[1]; - const FCL_REAL& C = ellipsoid.radii[2]; - - FCL_REAL Aa = A * a; - FCL_REAL Ab = A * b; - FCL_REAL Ba = B * a; - FCL_REAL Bb = B * b; - FCL_REAL Ca = C * a; - FCL_REAL Cb = C * b; - result[0] = tf.transform(Vec3f(0, Ba, Cb)); - result[1] = tf.transform(Vec3f(0, -Ba, Cb)); - result[2] = tf.transform(Vec3f(0, Ba, -Cb)); - result[3] = tf.transform(Vec3f(0, -Ba, -Cb)); - result[4] = tf.transform(Vec3f(Aa, Bb, 0)); - result[5] = tf.transform(Vec3f(-Aa, Bb, 0)); - result[6] = tf.transform(Vec3f(Aa, -Bb, 0)); - result[7] = tf.transform(Vec3f(-Aa, -Bb, 0)); - result[8] = tf.transform(Vec3f(Ab, 0, Ca)); - result[9] = tf.transform(Vec3f(Ab, 0, -Ca)); - result[10] = tf.transform(Vec3f(-Ab, 0, Ca)); - result[11] = tf.transform(Vec3f(-Ab, 0, -Ca)); +std::vector getBoundVertices(const Ellipsoid& ellipsoid, + const Transform3s& tf) { + std::vector result(12); + const CoalScalar phi = (1 + sqrt(5.0)) / 2.0; + + const CoalScalar a = sqrt(3.0) / (phi * phi); + const CoalScalar b = phi * a; + + const CoalScalar& A = ellipsoid.radii[0]; + const CoalScalar& B = ellipsoid.radii[1]; + const CoalScalar& C = ellipsoid.radii[2]; + + CoalScalar Aa = A * a; + CoalScalar Ab = A * b; + CoalScalar Ba = B * a; + CoalScalar Bb = B * b; + CoalScalar Ca = C * a; + CoalScalar Cb = C * b; + result[0] = tf.transform(Vec3s(0, Ba, Cb)); + result[1] = tf.transform(Vec3s(0, -Ba, Cb)); + result[2] = tf.transform(Vec3s(0, Ba, -Cb)); + result[3] = tf.transform(Vec3s(0, -Ba, -Cb)); + result[4] = tf.transform(Vec3s(Aa, Bb, 0)); + result[5] = tf.transform(Vec3s(-Aa, Bb, 0)); + result[6] = tf.transform(Vec3s(Aa, -Bb, 0)); + result[7] = tf.transform(Vec3s(-Aa, -Bb, 0)); + result[8] = tf.transform(Vec3s(Ab, 0, Ca)); + result[9] = tf.transform(Vec3s(Ab, 0, -Ca)); + result[10] = tf.transform(Vec3s(-Ab, 0, Ca)); + result[11] = tf.transform(Vec3s(-Ab, 0, -Ca)); return result; } -std::vector getBoundVertices(const Capsule& capsule, - const Transform3f& tf) { - std::vector result(36); - const FCL_REAL m = (1 + sqrt(5.0)) / 2.0; - - FCL_REAL hl = capsule.halfLength; - FCL_REAL edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0)); - FCL_REAL a = edge_size; - FCL_REAL b = m * edge_size; - FCL_REAL r2 = capsule.radius * 2 / sqrt(3.0); - - result[0] = tf.transform(Vec3f(0, a, b + hl)); - result[1] = tf.transform(Vec3f(0, -a, b + hl)); - result[2] = tf.transform(Vec3f(0, a, -b + hl)); - result[3] = tf.transform(Vec3f(0, -a, -b + hl)); - result[4] = tf.transform(Vec3f(a, b, hl)); - result[5] = tf.transform(Vec3f(-a, b, hl)); - result[6] = tf.transform(Vec3f(a, -b, hl)); - result[7] = tf.transform(Vec3f(-a, -b, hl)); - result[8] = tf.transform(Vec3f(b, 0, a + hl)); - result[9] = tf.transform(Vec3f(b, 0, -a + hl)); - result[10] = tf.transform(Vec3f(-b, 0, a + hl)); - result[11] = tf.transform(Vec3f(-b, 0, -a + hl)); - - result[12] = tf.transform(Vec3f(0, a, b - hl)); - result[13] = tf.transform(Vec3f(0, -a, b - hl)); - result[14] = tf.transform(Vec3f(0, a, -b - hl)); - result[15] = tf.transform(Vec3f(0, -a, -b - hl)); - result[16] = tf.transform(Vec3f(a, b, -hl)); - result[17] = tf.transform(Vec3f(-a, b, -hl)); - result[18] = tf.transform(Vec3f(a, -b, -hl)); - result[19] = tf.transform(Vec3f(-a, -b, -hl)); - result[20] = tf.transform(Vec3f(b, 0, a - hl)); - result[21] = tf.transform(Vec3f(b, 0, -a - hl)); - result[22] = tf.transform(Vec3f(-b, 0, a - hl)); - result[23] = tf.transform(Vec3f(-b, 0, -a - hl)); - - FCL_REAL c = 0.5 * r2; - FCL_REAL d = capsule.radius; - result[24] = tf.transform(Vec3f(r2, 0, hl)); - result[25] = tf.transform(Vec3f(c, d, hl)); - result[26] = tf.transform(Vec3f(-c, d, hl)); - result[27] = tf.transform(Vec3f(-r2, 0, hl)); - result[28] = tf.transform(Vec3f(-c, -d, hl)); - result[29] = tf.transform(Vec3f(c, -d, hl)); - - result[30] = tf.transform(Vec3f(r2, 0, -hl)); - result[31] = tf.transform(Vec3f(c, d, -hl)); - result[32] = tf.transform(Vec3f(-c, d, -hl)); - result[33] = tf.transform(Vec3f(-r2, 0, -hl)); - result[34] = tf.transform(Vec3f(-c, -d, -hl)); - result[35] = tf.transform(Vec3f(c, -d, -hl)); +std::vector getBoundVertices(const Capsule& capsule, + const Transform3s& tf) { + std::vector result(36); + const CoalScalar m = (1 + sqrt(5.0)) / 2.0; + + CoalScalar hl = capsule.halfLength; + CoalScalar edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0)); + CoalScalar a = edge_size; + CoalScalar b = m * edge_size; + CoalScalar r2 = capsule.radius * 2 / sqrt(3.0); + + result[0] = tf.transform(Vec3s(0, a, b + hl)); + result[1] = tf.transform(Vec3s(0, -a, b + hl)); + result[2] = tf.transform(Vec3s(0, a, -b + hl)); + result[3] = tf.transform(Vec3s(0, -a, -b + hl)); + result[4] = tf.transform(Vec3s(a, b, hl)); + result[5] = tf.transform(Vec3s(-a, b, hl)); + result[6] = tf.transform(Vec3s(a, -b, hl)); + result[7] = tf.transform(Vec3s(-a, -b, hl)); + result[8] = tf.transform(Vec3s(b, 0, a + hl)); + result[9] = tf.transform(Vec3s(b, 0, -a + hl)); + result[10] = tf.transform(Vec3s(-b, 0, a + hl)); + result[11] = tf.transform(Vec3s(-b, 0, -a + hl)); + + result[12] = tf.transform(Vec3s(0, a, b - hl)); + result[13] = tf.transform(Vec3s(0, -a, b - hl)); + result[14] = tf.transform(Vec3s(0, a, -b - hl)); + result[15] = tf.transform(Vec3s(0, -a, -b - hl)); + result[16] = tf.transform(Vec3s(a, b, -hl)); + result[17] = tf.transform(Vec3s(-a, b, -hl)); + result[18] = tf.transform(Vec3s(a, -b, -hl)); + result[19] = tf.transform(Vec3s(-a, -b, -hl)); + result[20] = tf.transform(Vec3s(b, 0, a - hl)); + result[21] = tf.transform(Vec3s(b, 0, -a - hl)); + result[22] = tf.transform(Vec3s(-b, 0, a - hl)); + result[23] = tf.transform(Vec3s(-b, 0, -a - hl)); + + CoalScalar c = 0.5 * r2; + CoalScalar d = capsule.radius; + result[24] = tf.transform(Vec3s(r2, 0, hl)); + result[25] = tf.transform(Vec3s(c, d, hl)); + result[26] = tf.transform(Vec3s(-c, d, hl)); + result[27] = tf.transform(Vec3s(-r2, 0, hl)); + result[28] = tf.transform(Vec3s(-c, -d, hl)); + result[29] = tf.transform(Vec3s(c, -d, hl)); + + result[30] = tf.transform(Vec3s(r2, 0, -hl)); + result[31] = tf.transform(Vec3s(c, d, -hl)); + result[32] = tf.transform(Vec3s(-c, d, -hl)); + result[33] = tf.transform(Vec3s(-r2, 0, -hl)); + result[34] = tf.transform(Vec3s(-c, -d, -hl)); + result[35] = tf.transform(Vec3s(c, -d, -hl)); return result; } -std::vector getBoundVertices(const Cone& cone, const Transform3f& tf) { - std::vector result(7); +std::vector getBoundVertices(const Cone& cone, const Transform3s& tf) { + std::vector result(7); - FCL_REAL hl = cone.halfLength; - FCL_REAL r2 = cone.radius * 2 / sqrt(3.0); - FCL_REAL a = 0.5 * r2; - FCL_REAL b = cone.radius; + CoalScalar hl = cone.halfLength; + CoalScalar r2 = cone.radius * 2 / sqrt(3.0); + CoalScalar a = 0.5 * r2; + CoalScalar b = cone.radius; - result[0] = tf.transform(Vec3f(r2, 0, -hl)); - result[1] = tf.transform(Vec3f(a, b, -hl)); - result[2] = tf.transform(Vec3f(-a, b, -hl)); - result[3] = tf.transform(Vec3f(-r2, 0, -hl)); - result[4] = tf.transform(Vec3f(-a, -b, -hl)); - result[5] = tf.transform(Vec3f(a, -b, -hl)); + result[0] = tf.transform(Vec3s(r2, 0, -hl)); + result[1] = tf.transform(Vec3s(a, b, -hl)); + result[2] = tf.transform(Vec3s(-a, b, -hl)); + result[3] = tf.transform(Vec3s(-r2, 0, -hl)); + result[4] = tf.transform(Vec3s(-a, -b, -hl)); + result[5] = tf.transform(Vec3s(a, -b, -hl)); - result[6] = tf.transform(Vec3f(0, 0, hl)); + result[6] = tf.transform(Vec3s(0, 0, hl)); return result; } -std::vector getBoundVertices(const Cylinder& cylinder, - const Transform3f& tf) { - std::vector result(12); - - FCL_REAL hl = cylinder.halfLength; - FCL_REAL r2 = cylinder.radius * 2 / sqrt(3.0); - FCL_REAL a = 0.5 * r2; - FCL_REAL b = cylinder.radius; - - result[0] = tf.transform(Vec3f(r2, 0, -hl)); - result[1] = tf.transform(Vec3f(a, b, -hl)); - result[2] = tf.transform(Vec3f(-a, b, -hl)); - result[3] = tf.transform(Vec3f(-r2, 0, -hl)); - result[4] = tf.transform(Vec3f(-a, -b, -hl)); - result[5] = tf.transform(Vec3f(a, -b, -hl)); - - result[6] = tf.transform(Vec3f(r2, 0, hl)); - result[7] = tf.transform(Vec3f(a, b, hl)); - result[8] = tf.transform(Vec3f(-a, b, hl)); - result[9] = tf.transform(Vec3f(-r2, 0, hl)); - result[10] = tf.transform(Vec3f(-a, -b, hl)); - result[11] = tf.transform(Vec3f(a, -b, hl)); +std::vector getBoundVertices(const Cylinder& cylinder, + const Transform3s& tf) { + std::vector result(12); + + CoalScalar hl = cylinder.halfLength; + CoalScalar r2 = cylinder.radius * 2 / sqrt(3.0); + CoalScalar a = 0.5 * r2; + CoalScalar b = cylinder.radius; + + result[0] = tf.transform(Vec3s(r2, 0, -hl)); + result[1] = tf.transform(Vec3s(a, b, -hl)); + result[2] = tf.transform(Vec3s(-a, b, -hl)); + result[3] = tf.transform(Vec3s(-r2, 0, -hl)); + result[4] = tf.transform(Vec3s(-a, -b, -hl)); + result[5] = tf.transform(Vec3s(a, -b, -hl)); + + result[6] = tf.transform(Vec3s(r2, 0, hl)); + result[7] = tf.transform(Vec3s(a, b, hl)); + result[8] = tf.transform(Vec3s(-a, b, hl)); + result[9] = tf.transform(Vec3s(-r2, 0, hl)); + result[10] = tf.transform(Vec3s(-a, -b, hl)); + result[11] = tf.transform(Vec3s(a, -b, hl)); return result; } -std::vector getBoundVertices(const ConvexBase& convex, - const Transform3f& tf) { - std::vector result(convex.num_points); - const std::vector& points_ = *(convex.points); +std::vector getBoundVertices(const ConvexBase& convex, + const Transform3s& tf) { + std::vector result(convex.num_points); + const std::vector& points_ = *(convex.points); for (std::size_t i = 0; i < convex.num_points; ++i) { result[i] = tf.transform(points_[i]); } @@ -234,9 +233,9 @@ std::vector getBoundVertices(const ConvexBase& convex, return result; } -std::vector getBoundVertices(const TriangleP& triangle, - const Transform3f& tf) { - std::vector result(3); +std::vector getBoundVertices(const TriangleP& triangle, + const Transform3s& tf) { + std::vector result(3); result[0] = tf.transform(triangle.a); result[1] = tf.transform(triangle.b); result[2] = tf.transform(triangle.c); @@ -246,30 +245,30 @@ std::vector getBoundVertices(const TriangleP& triangle, } // namespace details -Halfspace transform(const Halfspace& a, const Transform3f& tf) { +Halfspace transform(const Halfspace& a, const Transform3s& tf) { /// suppose the initial halfspace is n * x <= d /// after transform (R, T), x --> x' = R x + T /// and the new half space becomes n' * x' <= d' /// where n' = R * n /// and d' = d + n' * T - Vec3f n = tf.getRotation() * a.n; - FCL_REAL d = a.d + n.dot(tf.getTranslation()); + Vec3s n = tf.getRotation() * a.n; + CoalScalar d = a.d + n.dot(tf.getTranslation()); Halfspace result(n, d); result.setSweptSphereRadius(a.getSweptSphereRadius()); return result; } -Plane transform(const Plane& a, const Transform3f& tf) { +Plane transform(const Plane& a, const Transform3s& tf) { /// suppose the initial halfspace is n * x <= d /// after transform (R, T), x --> x' = R x + T /// and the new half space becomes n' * x' <= d' /// where n' = R * n /// and d' = d + n' * T - Vec3f n = tf.getRotation() * a.n; - FCL_REAL d = a.d + n.dot(tf.getTranslation()); + Vec3s n = tf.getRotation() * a.n; + CoalScalar d = a.d + n.dot(tf.getTranslation()); Plane result(n, d); result.setSweptSphereRadius(a.getSweptSphereRadius()); @@ -277,11 +276,11 @@ Plane transform(const Plane& a, const Transform3f& tf) { } std::array transformToHalfspaces(const Plane& a, - const Transform3f& tf) { + const Transform3s& tf) { // A plane can be represented by two halfspaces - Vec3f n = tf.getRotation() * a.n; - FCL_REAL d = a.d + n.dot(tf.getTranslation()); + Vec3s n = tf.getRotation() * a.n; + CoalScalar d = a.d + n.dot(tf.getTranslation()); std::array result = {Halfspace(n, d), Halfspace(-n, -d)}; result[0].setSweptSphereRadius(a.getSweptSphereRadius()); result[1].setSweptSphereRadius(a.getSweptSphereRadius()); @@ -290,91 +289,91 @@ std::array transformToHalfspaces(const Plane& a, } template <> -void computeBV(const Box& s, const Transform3f& tf, AABB& bv) { - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); +void computeBV(const Box& s, const Transform3s& tf, AABB& bv) { + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); - Vec3f v_delta(R.cwiseAbs() * s.halfSide); + Vec3s v_delta(R.cwiseAbs() * s.halfSide); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template <> -void computeBV(const Sphere& s, const Transform3f& tf, AABB& bv) { - const Vec3f& T = tf.getTranslation(); +void computeBV(const Sphere& s, const Transform3s& tf, AABB& bv) { + const Vec3s& T = tf.getTranslation(); - Vec3f v_delta(Vec3f::Constant(s.radius)); + Vec3s v_delta(Vec3s::Constant(s.radius)); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template <> -void computeBV(const Ellipsoid& e, const Transform3f& tf, +void computeBV(const Ellipsoid& e, const Transform3s& tf, AABB& bv) { - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); - Vec3f v_delta = R * e.radii; + Vec3s v_delta = R * e.radii; bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template <> -void computeBV(const Capsule& s, const Transform3f& tf, +void computeBV(const Capsule& s, const Transform3s& tf, AABB& bv) { - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); - Vec3f v_delta(R.col(2).cwiseAbs() * s.halfLength + Vec3f::Constant(s.radius)); + Vec3s v_delta(R.col(2).cwiseAbs() * s.halfLength + Vec3s::Constant(s.radius)); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template <> -void computeBV(const Cone& s, const Transform3f& tf, AABB& bv) { - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); - - FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + - fabs(R(0, 2) * s.halfLength); - FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + - fabs(R(1, 2) * s.halfLength); - FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + - fabs(R(2, 2) * s.halfLength); - - Vec3f v_delta(x_range, y_range, z_range); +void computeBV(const Cone& s, const Transform3s& tf, AABB& bv) { + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); + + CoalScalar x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + + fabs(R(0, 2) * s.halfLength); + CoalScalar y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + + fabs(R(1, 2) * s.halfLength); + CoalScalar z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + + fabs(R(2, 2) * s.halfLength); + + Vec3s v_delta(x_range, y_range, z_range); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template <> -void computeBV(const Cylinder& s, const Transform3f& tf, +void computeBV(const Cylinder& s, const Transform3s& tf, AABB& bv) { - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); - FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + - fabs(R(0, 2) * s.halfLength); - FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + - fabs(R(1, 2) * s.halfLength); - FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + - fabs(R(2, 2) * s.halfLength); + CoalScalar x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + + fabs(R(0, 2) * s.halfLength); + CoalScalar y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + + fabs(R(1, 2) * s.halfLength); + CoalScalar z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + + fabs(R(2, 2) * s.halfLength); - Vec3f v_delta(x_range, y_range, z_range); + Vec3s v_delta(x_range, y_range, z_range); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template <> -void computeBV(const ConvexBase& s, const Transform3f& tf, +void computeBV(const ConvexBase& s, const Transform3s& tf, AABB& bv) { - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); AABB bv_; - const std::vector& points_ = *(s.points); + const std::vector& points_ = *(s.points); for (std::size_t i = 0; i < s.num_points; ++i) { - Vec3f new_p = R * points_[i] + T; + Vec3s new_p = R * points_[i] + T; bv_ += new_p; } @@ -382,34 +381,34 @@ void computeBV(const ConvexBase& s, const Transform3f& tf, } template <> -void computeBV(const TriangleP& s, const Transform3f& tf, +void computeBV(const TriangleP& s, const Transform3s& tf, AABB& bv) { bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c)); } template <> -void computeBV(const Halfspace& s, const Transform3f& tf, +void computeBV(const Halfspace& s, const Transform3s& tf, AABB& bv) { Halfspace new_s = transform(s, tf); - const Vec3f& n = new_s.n; - const FCL_REAL& d = new_s.d; + const Vec3s& n = new_s.n; + const CoalScalar& d = new_s.d; AABB bv_; - bv_.min_ = Vec3f::Constant(-(std::numeric_limits::max)()); - bv_.max_ = Vec3f::Constant((std::numeric_limits::max)()); - if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + bv_.min_ = Vec3s::Constant(-(std::numeric_limits::max)()); + bv_.max_ = Vec3s::Constant((std::numeric_limits::max)()); + if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { // normal aligned with x axis if (n[0] < 0) bv_.min_[0] = -d; else if (n[0] > 0) bv_.max_[0] = d; - } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { // normal aligned with y axis if (n[1] < 0) bv_.min_[1] = -d; else if (n[1] > 0) bv_.max_[1] = d; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) { // normal aligned with z axis if (n[2] < 0) bv_.min_[2] = -d; @@ -421,29 +420,29 @@ void computeBV(const Halfspace& s, const Transform3f& tf, } template <> -void computeBV(const Plane& s, const Transform3f& tf, AABB& bv) { +void computeBV(const Plane& s, const Transform3s& tf, AABB& bv) { Plane new_s = transform(s, tf); - const Vec3f& n = new_s.n; - const FCL_REAL& d = new_s.d; + const Vec3s& n = new_s.n; + const CoalScalar& d = new_s.d; AABB bv_; - bv_.min_ = Vec3f::Constant(-(std::numeric_limits::max)()); - bv_.max_ = Vec3f::Constant((std::numeric_limits::max)()); - if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + bv_.min_ = Vec3s::Constant(-(std::numeric_limits::max)()); + bv_.max_ = Vec3s::Constant((std::numeric_limits::max)()); + if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { // normal aligned with x axis if (n[0] < 0) { bv_.min_[0] = bv_.max_[0] = -d; } else if (n[0] > 0) { bv_.min_[0] = bv_.max_[0] = d; } - } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { // normal aligned with y axis if (n[1] < 0) { bv_.min_[1] = bv_.max_[1] = -d; } else if (n[1] > 0) { bv_.min_[1] = bv_.max_[1] = d; } - } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) { // normal aligned with z axis if (n[2] < 0) { bv_.min_[2] = bv_.max_[2] = -d; @@ -456,13 +455,13 @@ void computeBV(const Plane& s, const Transform3f& tf, AABB& bv) { } template <> -void computeBV(const Box& s, const Transform3f& tf, OBB& bv) { +void computeBV(const Box& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); bv.To = T; bv.axes = R; @@ -470,12 +469,12 @@ void computeBV(const Box& s, const Transform3f& tf, OBB& bv) { } template <> -void computeBV(const Sphere& s, const Transform3f& tf, OBB& bv) { +void computeBV(const Sphere& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } - const Vec3f& T = tf.getTranslation(); + const Vec3s& T = tf.getTranslation(); bv.To.noalias() = T; bv.axes.setIdentity(); @@ -483,13 +482,13 @@ void computeBV(const Sphere& s, const Transform3f& tf, OBB& bv) { } template <> -void computeBV(const Capsule& s, const Transform3f& tf, OBB& bv) { +void computeBV(const Capsule& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); bv.To.noalias() = T; bv.axes.noalias() = R; @@ -497,13 +496,13 @@ void computeBV(const Capsule& s, const Transform3f& tf, OBB& bv) { } template <> -void computeBV(const Cone& s, const Transform3f& tf, OBB& bv) { +void computeBV(const Cone& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); bv.To.noalias() = T; bv.axes.noalias() = R; @@ -511,14 +510,14 @@ void computeBV(const Cone& s, const Transform3f& tf, OBB& bv) { } template <> -void computeBV(const Cylinder& s, const Transform3f& tf, +void computeBV(const Cylinder& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); bv.To.noalias() = T; bv.axes.noalias() = R; @@ -526,14 +525,14 @@ void computeBV(const Cylinder& s, const Transform3f& tf, } template <> -void computeBV(const ConvexBase& s, const Transform3f& tf, +void computeBV(const ConvexBase& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); fit(s.points->data(), s.num_points, bv); @@ -543,109 +542,109 @@ void computeBV(const ConvexBase& s, const Transform3f& tf, } template <> -void computeBV(const Halfspace& s, const Transform3f&, +void computeBV(const Halfspace& s, const Transform3s&, OBB& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } /// Half space can only have very rough OBB bv.axes.setIdentity(); bv.To.setZero(); - bv.extent.setConstant(((std::numeric_limits::max)())); + bv.extent.setConstant(((std::numeric_limits::max)())); } template <> -void computeBV(const Halfspace& s, const Transform3f&, +void computeBV(const Halfspace& s, const Transform3s&, RSS& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } /// Half space can only have very rough RSS bv.axes.setIdentity(); bv.Tr.setZero(); bv.length[0] = bv.length[1] = bv.radius = - (std::numeric_limits::max)(); + (std::numeric_limits::max)(); } template <> -void computeBV(const Halfspace& s, const Transform3f& tf, +void computeBV(const Halfspace& s, const Transform3s& tf, OBBRSS& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } computeBV(s, tf, bv.obb); computeBV(s, tf, bv.rss); } template <> -void computeBV(const Halfspace& s, const Transform3f& tf, +void computeBV(const Halfspace& s, const Transform3s& tf, kIOS& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } bv.num_spheres = 1; computeBV(s, tf, bv.obb); - bv.spheres[0].o = Vec3f(); - bv.spheres[0].r = (std::numeric_limits::max)(); + bv.spheres[0].o = Vec3s(); + bv.spheres[0].r = (std::numeric_limits::max)(); } template <> -void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, +void computeBV, Halfspace>(const Halfspace& s, const Transform3s& tf, KDOP<16>& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } Halfspace new_s = transform(s, tf); - const Vec3f& n = new_s.n; - const FCL_REAL& d = new_s.d; + const Vec3s& n = new_s.n; + const CoalScalar& d = new_s.d; const short D = 8; for (short i = 0; i < D; ++i) - bv.dist(i) = -(std::numeric_limits::max)(); + bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) - bv.dist(i) = (std::numeric_limits::max)(); + bv.dist(i) = (std::numeric_limits::max)(); - if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D) = d; else bv.dist(0) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(D + 1) = d; else bv.dist(1) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) { if (n[2] > 0) bv.dist(D + 2) = d; else bv.dist(2) = -d; - } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { + } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) { if (n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; else bv.dist(3) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { + } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) { if (n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; else bv.dist(4) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) { if (n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; else bv.dist(5) = n[1] * d * 2; - } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; else bv.dist(6) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; else @@ -654,64 +653,64 @@ void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, } template <> -void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, +void computeBV, Halfspace>(const Halfspace& s, const Transform3s& tf, KDOP<18>& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } Halfspace new_s = transform(s, tf); - const Vec3f& n = new_s.n; - const FCL_REAL& d = new_s.d; + const Vec3s& n = new_s.n; + const CoalScalar& d = new_s.d; const short D = 9; for (short i = 0; i < D; ++i) - bv.dist(i) = -(std::numeric_limits::max)(); + bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) - bv.dist(i) = (std::numeric_limits::max)(); + bv.dist(i) = (std::numeric_limits::max)(); - if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D) = d; else bv.dist(0) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(D + 1) = d; else bv.dist(1) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) { if (n[2] > 0) bv.dist(D + 2) = d; else bv.dist(2) = -d; - } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { + } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) { if (n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; else bv.dist(3) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { + } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) { if (n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; else bv.dist(4) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) { if (n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; else bv.dist(5) = n[1] * d * 2; - } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; else bv.dist(6) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; else bv.dist(7) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; else @@ -720,79 +719,79 @@ void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, } template <> -void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, +void computeBV, Halfspace>(const Halfspace& s, const Transform3s& tf, KDOP<24>& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } Halfspace new_s = transform(s, tf); - const Vec3f& n = new_s.n; - const FCL_REAL& d = new_s.d; + const Vec3s& n = new_s.n; + const CoalScalar& d = new_s.d; const short D = 12; for (short i = 0; i < D; ++i) - bv.dist(i) = -(std::numeric_limits::max)(); + bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) - bv.dist(i) = (std::numeric_limits::max)(); + bv.dist(i) = (std::numeric_limits::max)(); - if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D) = d; else bv.dist(0) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(D + 1) = d; else bv.dist(1) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) { if (n[2] > 0) bv.dist(D + 2) = d; else bv.dist(2) = -d; - } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { + } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) { if (n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; else bv.dist(3) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { + } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) { if (n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; else bv.dist(4) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) { if (n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; else bv.dist(5) = n[1] * d * 2; - } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; else bv.dist(6) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; else bv.dist(7) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; else bv.dist(8) = n[1] * d * 2; - } else if (n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + } else if (n[0] + n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D + 9) = n[0] * d * 3; else bv.dist(9) = n[0] * d * 3; - } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { + } else if (n[0] + n[1] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D + 10) = n[0] * d * 3; else bv.dist(10) = n[0] * d * 3; - } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + } else if (n[0] + n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(D + 11) = n[1] * d * 3; else @@ -801,302 +800,300 @@ void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, } template <> -void computeBV(const Plane& s, const Transform3f& tf, OBB& bv) { +void computeBV(const Plane& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } - Vec3f n = tf.getRotation() * s.n; + Vec3s n = tf.getRotation() * s.n; generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2)); bv.axes.col(0).noalias() = n; - bv.extent << 0, (std::numeric_limits::max)(), - (std::numeric_limits::max)(); + bv.extent << 0, (std::numeric_limits::max)(), + (std::numeric_limits::max)(); - Vec3f p = s.n * s.d; + Vec3s p = s.n * s.d; bv.To = tf.transform(p); /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T } template <> -void computeBV(const Plane& s, const Transform3f& tf, RSS& bv) { +void computeBV(const Plane& s, const Transform3s& tf, RSS& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } - Vec3f n = tf.getRotation() * s.n; + Vec3s n = tf.getRotation() * s.n; generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2)); bv.axes.col(0).noalias() = n; - bv.length[0] = (std::numeric_limits::max)(); - bv.length[1] = (std::numeric_limits::max)(); + bv.length[0] = (std::numeric_limits::max)(); + bv.length[1] = (std::numeric_limits::max)(); bv.radius = 0; - Vec3f p = s.n * s.d; + Vec3s p = s.n * s.d; bv.Tr = tf.transform(p); } template <> -void computeBV(const Plane& s, const Transform3f& tf, +void computeBV(const Plane& s, const Transform3s& tf, OBBRSS& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } computeBV(s, tf, bv.obb); computeBV(s, tf, bv.rss); } template <> -void computeBV(const Plane& s, const Transform3f& tf, kIOS& bv) { +void computeBV(const Plane& s, const Transform3s& tf, kIOS& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } bv.num_spheres = 1; computeBV(s, tf, bv.obb); - bv.spheres[0].o = Vec3f(); - bv.spheres[0].r = (std::numeric_limits::max)(); + bv.spheres[0].o = Vec3s(); + bv.spheres[0].r = (std::numeric_limits::max)(); } template <> -void computeBV, Plane>(const Plane& s, const Transform3f& tf, +void computeBV, Plane>(const Plane& s, const Transform3s& tf, KDOP<16>& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } Plane new_s = transform(s, tf); - const Vec3f& n = new_s.n; - const FCL_REAL& d = new_s.d; + const Vec3s& n = new_s.n; + const CoalScalar& d = new_s.d; const short D = 8; for (short i = 0; i < D; ++i) - bv.dist(i) = -(std::numeric_limits::max)(); + bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) - bv.dist(i) = (std::numeric_limits::max)(); + bv.dist(i) = (std::numeric_limits::max)(); - if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(0) = bv.dist(D) = d; else bv.dist(0) = bv.dist(D) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; else bv.dist(1) = bv.dist(D + 1) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) { if (n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; else bv.dist(2) = bv.dist(D + 2) = -d; - } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { + } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) { bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { + } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) { bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) { bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2; - } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) { bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) { bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; } } template <> -void computeBV, Plane>(const Plane& s, const Transform3f& tf, +void computeBV, Plane>(const Plane& s, const Transform3s& tf, KDOP<18>& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } Plane new_s = transform(s, tf); - const Vec3f& n = new_s.n; - const FCL_REAL& d = new_s.d; + const Vec3s& n = new_s.n; + const CoalScalar& d = new_s.d; const short D = 9; for (short i = 0; i < D; ++i) - bv.dist(i) = -(std::numeric_limits::max)(); + bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) - bv.dist(i) = (std::numeric_limits::max)(); + bv.dist(i) = (std::numeric_limits::max)(); - if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(0) = bv.dist(D) = d; else bv.dist(0) = bv.dist(D) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; else bv.dist(1) = bv.dist(D + 1) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) { if (n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; else bv.dist(2) = bv.dist(D + 2) = -d; - } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { + } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) { bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { + } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) { bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) { bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; - } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) { bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) { bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) { bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; } } template <> -void computeBV, Plane>(const Plane& s, const Transform3f& tf, +void computeBV, Plane>(const Plane& s, const Transform3s& tf, KDOP<24>& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } Plane new_s = transform(s, tf); - const Vec3f& n = new_s.n; - const FCL_REAL& d = new_s.d; + const Vec3s& n = new_s.n; + const CoalScalar& d = new_s.d; const short D = 12; for (short i = 0; i < D; ++i) - bv.dist(i) = -(std::numeric_limits::max)(); + bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) - bv.dist(i) = (std::numeric_limits::max)(); + bv.dist(i) = (std::numeric_limits::max)(); - if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(0) = bv.dist(D) = d; else bv.dist(0) = bv.dist(D) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; else bv.dist(1) = bv.dist(D + 1) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) { if (n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; else bv.dist(2) = bv.dist(D + 2) = -d; - } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { + } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) { bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { + } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) { bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) { bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; - } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) { bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) { bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) { bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; - } else if (n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + } else if (n[0] + n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) { bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3; - } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { + } else if (n[0] + n[1] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) { bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3; - } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + } else if (n[0] + n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) { bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3; } } -void constructBox(const AABB& bv, Box& box, Transform3f& tf) { +void constructBox(const AABB& bv, Box& box, Transform3s& tf) { box = Box(bv.max_ - bv.min_); - tf = Transform3f(bv.center()); + tf = Transform3s(bv.center()); } -void constructBox(const OBB& bv, Box& box, Transform3f& tf) { +void constructBox(const OBB& bv, Box& box, Transform3s& tf) { box = Box(bv.extent * 2); - tf = Transform3f(bv.axes, bv.To); + tf = Transform3s(bv.axes, bv.To); } -void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf) { +void constructBox(const OBBRSS& bv, Box& box, Transform3s& tf) { box = Box(bv.obb.extent * 2); - tf = Transform3f(bv.obb.axes, bv.obb.To); + tf = Transform3s(bv.obb.axes, bv.obb.To); } -void constructBox(const kIOS& bv, Box& box, Transform3f& tf) { +void constructBox(const kIOS& bv, Box& box, Transform3s& tf) { box = Box(bv.obb.extent * 2); - tf = Transform3f(bv.obb.axes, bv.obb.To); + tf = Transform3s(bv.obb.axes, bv.obb.To); } -void constructBox(const RSS& bv, Box& box, Transform3f& tf) { +void constructBox(const RSS& bv, Box& box, Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); - tf = Transform3f(bv.axes, bv.Tr); + tf = Transform3s(bv.axes, bv.Tr); } -void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf) { +void constructBox(const KDOP<16>& bv, Box& box, Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); - tf = Transform3f(bv.center()); + tf = Transform3s(bv.center()); } -void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf) { +void constructBox(const KDOP<18>& bv, Box& box, Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); - tf = Transform3f(bv.center()); + tf = Transform3s(bv.center()); } -void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf) { +void constructBox(const KDOP<24>& bv, Box& box, Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); - tf = Transform3f(bv.center()); + tf = Transform3s(bv.center()); } -void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf) { +void constructBox(const AABB& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf) { box = Box(bv.max_ - bv.min_); - tf = tf_bv * Transform3f(bv.center()); + tf = tf_bv * Transform3s(bv.center()); } -void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf) { +void constructBox(const OBB& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf) { box = Box(bv.extent * 2); - tf = tf_bv * Transform3f(bv.axes, bv.To); + tf = tf_bv * Transform3s(bv.axes, bv.To); } -void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf) { +void constructBox(const OBBRSS& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf) { box = Box(bv.obb.extent * 2); - tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To); + tf = tf_bv * Transform3s(bv.obb.axes, bv.obb.To); } -void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf) { +void constructBox(const kIOS& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf) { box = Box(bv.obb.extent * 2); - tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To); + tf = tf_bv * Transform3s(bv.obb.axes, bv.obb.To); } -void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf) { +void constructBox(const RSS& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); - tf = tf_bv * Transform3f(bv.axes, bv.Tr); + tf = tf_bv * Transform3s(bv.axes, bv.Tr); } -void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf) { +void constructBox(const KDOP<16>& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); - tf = tf_bv * Transform3f(bv.center()); + tf = tf_bv * Transform3s(bv.center()); } -void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf) { +void constructBox(const KDOP<18>& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); - tf = tf_bv * Transform3f(bv.center()); + tf = tf_bv * Transform3s(bv.center()); } -void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf) { +void constructBox(const KDOP<24>& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); - tf = tf_bv * Transform3f(bv.center()); + tf = tf_bv * Transform3s(bv.center()); } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/traits_traversal.h b/src/traits_traversal.h index 73282d36d..5667a69c6 100644 --- a/src/traits_traversal.h +++ b/src/traits_traversal.h @@ -7,54 +7,53 @@ /** \author Lucile Remigy, Joseph Mirabel */ -#include -#include +#include "coal/collision_func_matrix.h" +#include "coal/narrowphase/narrowphase.h" #include <../src/collision_node.h> -#include -#include +#include "coal/internal/traversal_node_setup.h" +#include "coal/internal/shape_shape_func.h" -namespace hpp { -namespace fcl { +namespace coal { // TraversalTraitsCollision for collision_func_matrix.cpp template -struct HPP_FCL_LOCAL TraversalTraitsCollision{}; +struct COAL_LOCAL TraversalTraitsCollision{}; -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP template -struct HPP_FCL_LOCAL TraversalTraitsCollision { +struct COAL_LOCAL TraversalTraitsCollision { typedef ShapeOcTreeCollisionTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsCollision { +struct COAL_LOCAL TraversalTraitsCollision { typedef OcTreeShapeCollisionTraversalNode CollisionTraversal_t; }; template <> -struct HPP_FCL_LOCAL TraversalTraitsCollision { +struct COAL_LOCAL TraversalTraitsCollision { typedef OcTreeCollisionTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsCollision > { +struct COAL_LOCAL TraversalTraitsCollision > { typedef OcTreeMeshCollisionTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsCollision, OcTree> { +struct COAL_LOCAL TraversalTraitsCollision, OcTree> { typedef MeshOcTreeCollisionTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsCollision > { +struct COAL_LOCAL TraversalTraitsCollision > { typedef OcTreeHeightFieldCollisionTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsCollision, OcTree> { +struct COAL_LOCAL TraversalTraitsCollision, OcTree> { typedef HeightFieldOcTreeCollisionTraversalNode CollisionTraversal_t; }; @@ -63,37 +62,35 @@ struct HPP_FCL_LOCAL TraversalTraitsCollision, OcTree> { // TraversalTraitsDistance for distance_func_matrix.cpp template -struct HPP_FCL_LOCAL TraversalTraitsDistance{}; +struct COAL_LOCAL TraversalTraitsDistance{}; -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP template -struct HPP_FCL_LOCAL TraversalTraitsDistance { +struct COAL_LOCAL TraversalTraitsDistance { typedef ShapeOcTreeDistanceTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsDistance { +struct COAL_LOCAL TraversalTraitsDistance { typedef OcTreeShapeDistanceTraversalNode CollisionTraversal_t; }; template <> -struct HPP_FCL_LOCAL TraversalTraitsDistance { +struct COAL_LOCAL TraversalTraitsDistance { typedef OcTreeDistanceTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsDistance > { +struct COAL_LOCAL TraversalTraitsDistance > { typedef OcTreeMeshDistanceTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsDistance, OcTree> { +struct COAL_LOCAL TraversalTraitsDistance, OcTree> { typedef MeshOcTreeDistanceTraversalNode CollisionTraversal_t; }; #endif -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/traversal/traversal_recurse.cpp b/src/traversal/traversal_recurse.cpp index 082b6c31a..2b133c5a6 100644 --- a/src/traversal/traversal_recurse.cpp +++ b/src/traversal/traversal_recurse.cpp @@ -35,16 +35,15 @@ /** \author Jia Pan */ -#include +#include "coal/internal/traversal_recurse.h" #include -namespace hpp { -namespace fcl { +namespace coal { void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1, unsigned int b2, BVHFrontList* front_list, - FCL_REAL& sqrDistLowerBound) { - FCL_REAL sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0; + CoalScalar& sqrDistLowerBound) { + CoalScalar sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0; bool l1 = node->isFirstNodeLeaf(b1); bool l2 = node->isSecondNodeLeaf(b2); if (l1 && l2) { @@ -86,15 +85,15 @@ void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1, void collisionNonRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list, - FCL_REAL& sqrDistLowerBound) { + CoalScalar& sqrDistLowerBound) { typedef std::pair BVPair_t; // typedef std::stack > Stack_t; typedef std::vector Stack_t; Stack_t pairs; pairs.reserve(1000); - sqrDistLowerBound = std::numeric_limits::infinity(); - FCL_REAL sdlb = std::numeric_limits::infinity(); + sqrDistLowerBound = std::numeric_limits::infinity(); + CoalScalar sdlb = std::numeric_limits::infinity(); pairs.push_back(BVPair_t(0, 0)); @@ -176,8 +175,8 @@ void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1, c2 = (unsigned int)node->getSecondRightChild(b2); } - FCL_REAL d1 = node->BVDistanceLowerBound(a1, a2); - FCL_REAL d2 = node->BVDistanceLowerBound(c1, c2); + CoalScalar d1 = node->BVDistanceLowerBound(a1, a2); + CoalScalar d2 = node->BVDistanceLowerBound(c1, c2); if (d2 < d1) { if (!node->canStop(d2)) @@ -203,21 +202,22 @@ void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1, } /** @brief Bounding volume test structure */ -struct HPP_FCL_LOCAL BVT { +struct COAL_LOCAL BVT { /** @brief distance between bvs */ - FCL_REAL d; + CoalScalar d; /** @brief bv indices for a pair of bvs in two models */ unsigned int b1, b2; }; /** @brief Comparer between two BVT */ -struct HPP_FCL_LOCAL BVT_Comparer{ - bool operator()(const BVT& lhs, const BVT& rhs) const {return lhs.d > rhs.d; -} // namespace fcl -}; // namespace hpp +struct COAL_LOCAL BVT_Comparer{bool operator()(const BVT& lhs, const BVT& rhs) + const {return lhs.d > rhs.d; +} // namespace coal +} +; -struct HPP_FCL_LOCAL BVTQ { +struct COAL_LOCAL BVTQ { BVTQ() : qsize(2) {} bool empty() const { return pq.empty(); } @@ -308,8 +308,8 @@ void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, const CollisionRequest& /*request*/, CollisionResult& result, BVHFrontList* front_list) { - FCL_REAL sqrDistLowerBound = -1, sqrDistLowerBound1 = 0, - sqrDistLowerBound2 = 0; + CoalScalar sqrDistLowerBound = -1, sqrDistLowerBound1 = 0, + sqrDistLowerBound2 = 0; BVHFrontList::iterator front_iter; BVHFrontList append; for (front_iter = front_list->begin(); front_iter != front_list->end(); @@ -359,6 +359,4 @@ void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, } } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 2891ad789..c85077bb0 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -3,80 +3,83 @@ FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework filesystem) config_files(fcl_resources/config.h) -macro(add_fcl_test test_name source) - ADD_UNIT_TEST(${test_name} ${source}) - target_link_libraries(${test_name} +function(add_coal_test test_name source) + set(target_name ${PROJECT_NAME}-${test_name}) + ADD_UNIT_TEST(${target_name} ${source}) + target_link_libraries(${target_name} PUBLIC - hpp-fcl + ${LIBRARY_NAME} Boost::filesystem - utility + ${utility_target} ) IF(NOT WIN32) - target_compile_options(${test_name} PRIVATE "-Wno-c99-extensions") - ENDIF(NOT WIN32) - if(HPP_FCL_HAS_QHULL) - target_compile_options(${test_name} PRIVATE -DHPP_FCL_HAS_QHULL) + target_compile_options(${target_name} PRIVATE "-Wno-c99-extensions") + ENDIF() + if(COAL_HAS_QHULL) + target_compile_options(${target_name} PRIVATE -DCOAL_HAS_QHULL) endif() -endmacro(add_fcl_test) +endfunction() include_directories(${CMAKE_CURRENT_BINARY_DIR}) -add_library(utility STATIC utility.cpp) -target_link_libraries(utility PUBLIC ${PROJECT_NAME}) +set(utility_target ${PROJECT_NAME}-utility) +add_library(${utility_target} STATIC utility.cpp) +target_link_libraries(${utility_target} PUBLIC ${PROJECT_NAME}) -add_fcl_test(math math.cpp) +add_coal_test(math math.cpp) -add_fcl_test(collision collision.cpp) -add_fcl_test(contact_patch contact_patch.cpp) -add_fcl_test(distance distance.cpp) -add_fcl_test(swept_sphere_radius swept_sphere_radius.cpp) -add_fcl_test(normal_and_nearest_points normal_and_nearest_points.cpp) -add_fcl_test(distance_lower_bound distance_lower_bound.cpp) -add_fcl_test(security_margin security_margin.cpp) -add_fcl_test(geometric_shapes geometric_shapes.cpp) -add_fcl_test(shape_inflation shape_inflation.cpp) -#add_fcl_test(shape_mesh_consistency shape_mesh_consistency.cpp) -add_fcl_test(gjk_asserts gjk_asserts.cpp) -add_fcl_test(frontlist frontlist.cpp) -SET_TESTS_PROPERTIES(frontlist PROPERTIES TIMEOUT 7200) +add_coal_test(collision collision.cpp) +add_coal_test(contact_patch contact_patch.cpp) +add_coal_test(distance distance.cpp) +add_coal_test(swept_sphere_radius swept_sphere_radius.cpp) +add_coal_test(normal_and_nearest_points normal_and_nearest_points.cpp) +add_coal_test(distance_lower_bound distance_lower_bound.cpp) +add_coal_test(security_margin security_margin.cpp) +add_coal_test(geometric_shapes geometric_shapes.cpp) +add_coal_test(shape_inflation shape_inflation.cpp) +#add_coal_test(shape_mesh_consistency shape_mesh_consistency.cpp) +add_coal_test(gjk_asserts gjk_asserts.cpp) +add_coal_test(frontlist frontlist.cpp) +SET_TESTS_PROPERTIES(${PROJECT_NAME}-frontlist PROPERTIES TIMEOUT 7200) -# add_fcl_test(sphere_capsule sphere_capsule.cpp) -add_fcl_test(capsule_capsule capsule_capsule.cpp) -add_fcl_test(box_box_distance box_box_distance.cpp) -add_fcl_test(box_box_collision box_box_collision.cpp) -add_fcl_test(simple simple.cpp) -add_fcl_test(capsule_box_1 capsule_box_1.cpp) -add_fcl_test(capsule_box_2 capsule_box_2.cpp) -add_fcl_test(obb obb.cpp) -add_fcl_test(convex convex.cpp) +# add_coal_test(sphere_capsule sphere_capsule.cpp) +add_coal_test(capsule_capsule capsule_capsule.cpp) +add_coal_test(box_box_distance box_box_distance.cpp) +add_coal_test(box_box_collision box_box_collision.cpp) +add_coal_test(simple simple.cpp) +add_coal_test(capsule_box_1 capsule_box_1.cpp) +add_coal_test(capsule_box_2 capsule_box_2.cpp) +add_coal_test(obb obb.cpp) +add_coal_test(convex convex.cpp) -add_fcl_test(bvh_models bvh_models.cpp) -add_fcl_test(collision_node_asserts collision_node_asserts.cpp) -add_fcl_test(hfields hfields.cpp) +add_coal_test(bvh_models bvh_models.cpp) +add_coal_test(collision_node_asserts collision_node_asserts.cpp) +add_coal_test(hfields hfields.cpp) -add_fcl_test(profiling profiling.cpp) +add_coal_test(profiling profiling.cpp) -add_fcl_test(gjk gjk.cpp) -add_fcl_test(accelerated_gjk accelerated_gjk.cpp) -add_fcl_test(gjk_convergence_criterion gjk_convergence_criterion.cpp) -if(HPP_FCL_HAS_OCTOMAP) - add_fcl_test(octree octree.cpp) -endif(HPP_FCL_HAS_OCTOMAP) +add_coal_test(gjk gjk.cpp) +add_coal_test(accelerated_gjk accelerated_gjk.cpp) +add_coal_test(gjk_convergence_criterion gjk_convergence_criterion.cpp) +if(COAL_HAS_OCTOMAP) + add_coal_test(octree octree.cpp) +endif(COAL_HAS_OCTOMAP) -add_fcl_test(serialization serialization.cpp) +add_coal_test(serialization serialization.cpp) # Broadphase -add_fcl_test(broadphase broadphase.cpp) -set_tests_properties(broadphase PROPERTIES WILL_FAIL TRUE) -add_fcl_test(broadphase_dynamic_AABB_tree broadphase_dynamic_AABB_tree.cpp) -add_fcl_test(broadphase_collision_1 broadphase_collision_1.cpp) -add_fcl_test(broadphase_collision_2 broadphase_collision_2.cpp) +add_coal_test(broadphase broadphase.cpp) +set_tests_properties(${PROJECT_NAME}-broadphase PROPERTIES WILL_FAIL TRUE) +add_coal_test(broadphase_dynamic_AABB_tree broadphase_dynamic_AABB_tree.cpp) +add_coal_test(broadphase_collision_1 broadphase_collision_1.cpp) +add_coal_test(broadphase_collision_2 broadphase_collision_2.cpp) ## Benchmark -add_executable(test-benchmark benchmark.cpp) -target_link_libraries(test-benchmark +set(test_benchmark_target ${PROJECT_NAME}-test-benchmark) +add_executable(${test_benchmark_target} benchmark.cpp) +target_link_libraries(${test_benchmark_target} PUBLIC - utility + ${utility_target} Boost::filesystem ${PROJECT_NAME} ) diff --git a/test/accelerated_gjk.cpp b/test/accelerated_gjk.cpp index 67d430d2f..367b49aa4 100644 --- a/test/accelerated_gjk.cpp +++ b/test/accelerated_gjk.cpp @@ -34,32 +34,32 @@ /** \author Louis Montaut */ -#define BOOST_TEST_MODULE FCL_NESTEROV_GJK +#define BOOST_TEST_MODULE COAL_NESTEROV_GJK #include #include -#include -#include -#include +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/internal/tools.h" #include "utility.h" -using hpp::fcl::Box; -using hpp::fcl::Capsule; -using hpp::fcl::constructPolytopeFromEllipsoid; -using hpp::fcl::Convex; -using hpp::fcl::Ellipsoid; -using hpp::fcl::FCL_REAL; -using hpp::fcl::GJKSolver; -using hpp::fcl::GJKVariant; -using hpp::fcl::ShapeBase; -using hpp::fcl::support_func_guess_t; -using hpp::fcl::Transform3f; -using hpp::fcl::Triangle; -using hpp::fcl::Vec3f; -using hpp::fcl::details::GJK; -using hpp::fcl::details::MinkowskiDiff; -using hpp::fcl::details::SupportOptions; +using coal::Box; +using coal::Capsule; +using coal::CoalScalar; +using coal::constructPolytopeFromEllipsoid; +using coal::Convex; +using coal::Ellipsoid; +using coal::GJKSolver; +using coal::GJKVariant; +using coal::ShapeBase; +using coal::support_func_guess_t; +using coal::Transform3s; +using coal::Triangle; +using coal::Vec3s; +using coal::details::GJK; +using coal::details::MinkowskiDiff; +using coal::details::SupportOptions; using std::size_t; BOOST_AUTO_TEST_CASE(set_gjk_variant) { @@ -107,7 +107,7 @@ BOOST_AUTO_TEST_CASE(need_nesterov_normalize_support_direction) { void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) { // Solvers unsigned int max_iterations = 128; - FCL_REAL tolerance = 1e-6; + CoalScalar tolerance = 1e-6; GJK gjk(max_iterations, tolerance); GJK gjk_nesterov(max_iterations, tolerance); gjk_nesterov.gjk_variant = GJKVariant::NesterovAcceleration; @@ -119,13 +119,13 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) { // Generate random transforms size_t n = 1000; - FCL_REAL extents[] = {-3., -3., 0, 3., 3., 3.}; - std::vector transforms; + CoalScalar extents[] = {-3., -3., 0, 3., 3., 3.}; + std::vector transforms; generateRandomTransforms(extents, transforms, n); - Transform3f identity = Transform3f::Identity(); + Transform3s identity = Transform3s::Identity(); // Same init for both solvers - Vec3f init_guess = Vec3f(1, 0, 0); + Vec3s init_guess = Vec3s(1, 0, 0); support_func_guess_t init_support_guess; init_support_guess.setZero(); @@ -139,7 +139,7 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) { // Evaluate both solvers twice, make sure they give the same solution GJK::Status res_gjk_1 = gjk.evaluate(mink_diff, init_guess, init_support_guess); - Vec3f ray_gjk = gjk.ray; + Vec3s ray_gjk = gjk.ray; GJK::Status res_gjk_2 = gjk.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(res_gjk_1 == res_gjk_2); @@ -150,7 +150,7 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) { // -------------- GJK::Status res_nesterov_gjk_1 = gjk_nesterov.evaluate(mink_diff, init_guess, init_support_guess); - Vec3f ray_nesterov = gjk_nesterov.ray; + Vec3s ray_nesterov = gjk_nesterov.ray; GJK::Status res_nesterov_gjk_2 = gjk_nesterov.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(res_nesterov_gjk_1 == res_nesterov_gjk_2); @@ -171,7 +171,7 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) { // ------------ GJK::Status res_polyak_gjk_1 = gjk_polyak.evaluate(mink_diff, init_guess, init_support_guess); - Vec3f ray_polyak = gjk_polyak.ray; + Vec3s ray_polyak = gjk_polyak.ray; GJK::Status res_polyak_gjk_2 = gjk_polyak.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(res_polyak_gjk_1 == res_polyak_gjk_2); diff --git a/test/benchmark.cpp b/test/benchmark.cpp index 54a22e5b3..fd1e973c4 100644 --- a/test/benchmark.cpp +++ b/test/benchmark.cpp @@ -1,25 +1,25 @@ // Copyright (c) 2016, Joseph Mirabel // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // -// This file is part of hpp-fcl. -// hpp-fcl is free software: you can redistribute it +// This file is part of Coal. +// Coal is free software: you can redistribute it // and/or modify it under the terms of the GNU Lesser General Public // License as published by the Free Software Foundation, either version // 3 of the License, or (at your option) any later version. // -// hpp-fcl is distributed in the hope that it will be +// Coal is distributed in the hope that it will be // useful, but WITHOUT ANY WARRANTY; without even the implied warranty // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Lesser Public License for more details. You should have // received a copy of the GNU Lesser General Public License along with -// hpp-fcl. If not, see . +// Coal. If not, see . #include -#include -#include +#include "coal/internal/traversal_node_setup.h" +#include "coal/internal/traversal_node_bvhs.h" #include "../src/collision_node.h" -#include +#include "coal/internal/BV_splitter.h" #include "utility.h" #include "fcl_resources/config.h" @@ -27,26 +27,26 @@ #define RUN_CASE(BV, tf, models, split) \ run(tf, models, split, #BV " - " #split ":\t") -using namespace hpp::fcl; +using namespace coal; bool verbose = false; -FCL_REAL DELTA = 0.001; +CoalScalar DELTA = 0.001; template -void makeModel(const std::vector& vertices, +void makeModel(const std::vector& vertices, const std::vector& triangles, SplitMethodType split_method, BVHModel& model); template -double distance(const std::vector& tf, const BVHModel& m1, +double distance(const std::vector& tf, const BVHModel& m1, const BVHModel& m2, bool verbose); template -double collide(const std::vector& tf, const BVHModel& m1, +double collide(const std::vector& tf, const BVHModel& m1, const BVHModel& m2, bool verbose); template -double run(const std::vector& tf, +double run(const std::vector& tf, const BVHModel (&models)[2][3], int split_method, const char* sm_name); @@ -78,7 +78,7 @@ struct traits { }; template -void makeModel(const std::vector& vertices, +void makeModel(const std::vector& vertices, const std::vector& triangles, SplitMethodType split_method, BVHModel& model) { model.bv_splitter.reset(new BVSplitter(split_method)); @@ -90,9 +90,9 @@ void makeModel(const std::vector& vertices, } template -double distance(const std::vector& tf, const BVHModel& m1, +double distance(const std::vector& tf, const BVHModel& m1, const BVHModel& m2, bool verbose) { - Transform3f pose2; + Transform3s pose2; DistanceResult local_result; DistanceRequest request(true); @@ -114,9 +114,9 @@ double distance(const std::vector& tf, const BVHModel& m1, } template -double collide(const std::vector& tf, const BVHModel& m1, +double collide(const std::vector& tf, const BVHModel& m1, const BVHModel& m2, bool verbose) { - Transform3f pose2; + Transform3s pose2; CollisionResult local_result; CollisionRequest request; @@ -141,7 +141,7 @@ double collide(const std::vector& tf, const BVHModel& m1, } template -double run(const std::vector& tf, +double run(const std::vector& tf, const BVHModel (&models)[2][3], int split_method, const char* prefix) { double col = collide::CollisionTraversalNode>( @@ -154,7 +154,7 @@ double run(const std::vector& tf, } template <> -double run(const std::vector& tf, +double run(const std::vector& tf, const BVHModel (&models)[2][3], int split_method, const char* prefix) { double col = collide::CollisionTraversalNode>( @@ -166,7 +166,7 @@ double run(const std::vector& tf, } int main(int, char*[]) { - std::vector p1, p2; + std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); @@ -207,8 +207,8 @@ int main(int, char*[]) { ms_obbrss[1][SPLIT_METHOD_BV_CENTER]); makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_obbrss[1][SPLIT_METHOD_MEDIAN]); - std::vector transforms; // t0 - FCL_REAL extents[] = {-3000, -3000, -3000, 3000, 3000, 3000}; + std::vector transforms; // t0 + CoalScalar extents[] = {-3000, -3000, -3000, 3000, 3000, 3000}; std::size_t n = 10000; generateRandomTransforms(extents, transforms, n); diff --git a/test/box_box_collision.cpp b/test/box_box_collision.cpp index b3276a18d..63c1f4fcf 100644 --- a/test/box_box_collision.cpp +++ b/test/box_box_collision.cpp @@ -1,21 +1,21 @@ -#define BOOST_TEST_MODULE BOX_BOX_COLLISION +#define BOOST_TEST_MODULE COAL_BOX_BOX_COLLISION #include #include -#include -#include -#include +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/internal/tools.h" #include "utility.h" -using hpp::fcl::Box; -using hpp::fcl::collide; -using hpp::fcl::CollisionRequest; -using hpp::fcl::CollisionResult; -using hpp::fcl::ComputeCollision; -using hpp::fcl::FCL_REAL; -using hpp::fcl::Transform3f; -using hpp::fcl::Vec3f; +using coal::Box; +using coal::CoalScalar; +using coal::collide; +using coal::CollisionRequest; +using coal::CollisionResult; +using coal::ComputeCollision; +using coal::Transform3s; +using coal::Vec3s; BOOST_AUTO_TEST_CASE(box_box_collision) { // Define boxes @@ -23,8 +23,8 @@ BOOST_AUTO_TEST_CASE(box_box_collision) { Box shape2(1, 1, 1); // Define transforms - Transform3f T1 = Transform3f::Identity(); - Transform3f T2 = Transform3f::Identity(); + Transform3s T1 = Transform3s::Identity(); + Transform3s T2 = Transform3s::Identity(); // Compute collision CollisionRequest req; @@ -33,13 +33,13 @@ BOOST_AUTO_TEST_CASE(box_box_collision) { CollisionResult res; ComputeCollision collide_functor(&shape1, &shape2); - T1.setTranslation(Vec3f(0, 0, 0)); + T1.setTranslation(Vec3s(0, 0, 0)); res.clear(); BOOST_CHECK(collide(&shape1, T1, &shape2, T2, req, res) == true); res.clear(); BOOST_CHECK(collide_functor(T1, T2, req, res) == true); - T1.setTranslation(Vec3f(2, 0, 0)); + T1.setTranslation(Vec3s(2, 0, 0)); res.clear(); BOOST_CHECK(collide(&shape1, T1, &shape2, T2, req, res) == false); res.clear(); diff --git a/test/box_box_distance.cpp b/test/box_box_distance.cpp index d0c272cd4..82ff5bfee 100644 --- a/test/box_box_distance.cpp +++ b/test/box_box_distance.cpp @@ -37,34 +37,34 @@ #define _USE_MATH_DEFINES #include -#define BOOST_TEST_MODULE FCL_BOX_BOX +#define BOOST_TEST_MODULE COAL_BOX_BOX #include #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) #include #include -#include -#include -#include -#include -#include +#include "coal/distance.h" +#include "coal/math/transform.h" +#include "coal/collision.h" +#include "coal/collision_object.h" +#include "coal/shape/geometric_shapes.h" #include "utility.h" -using hpp::fcl::CollisionGeometryPtr_t; -using hpp::fcl::CollisionObject; -using hpp::fcl::DistanceRequest; -using hpp::fcl::DistanceResult; -using hpp::fcl::Transform3f; -using hpp::fcl::Vec3f; +using coal::CollisionGeometryPtr_t; +using coal::CollisionObject; +using coal::DistanceRequest; +using coal::DistanceResult; +using coal::Transform3s; +using coal::Vec3s; BOOST_AUTO_TEST_CASE(distance_box_box_1) { - CollisionGeometryPtr_t s1(new hpp::fcl::Box(6, 10, 2)); - CollisionGeometryPtr_t s2(new hpp::fcl::Box(2, 2, 2)); + CollisionGeometryPtr_t s1(new coal::Box(6, 10, 2)); + CollisionGeometryPtr_t s2(new coal::Box(2, 2, 2)); - Transform3f tf1; - Transform3f tf2(Vec3f(25, 20, 5)); + Transform3s tf1; + Transform3s tf2(Vec3s(25, 20, 5)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -73,7 +73,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_1) { DistanceRequest distanceRequest(true, true, 0, 0); DistanceResult distanceResult; - hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult); + coal::distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied transformations on two boxes" << std::endl; std::cerr << " T1 = " << tf1.getTranslation() << std::endl @@ -87,8 +87,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_1) { double dy = 20 - 5 - 1; double dz = 5 - 1 - 1; - const Vec3f& p1 = distanceResult.nearest_points[0]; - const Vec3f& p2 = distanceResult.nearest_points[1]; + const Vec3s& p1 = distanceResult.nearest_points[0]; + const Vec3s& p2 = distanceResult.nearest_points[1]; BOOST_CHECK_CLOSE(distanceResult.min_distance, sqrt(dx * dx + dy * dy + dz * dz), 1e-4); @@ -101,14 +101,13 @@ BOOST_AUTO_TEST_CASE(distance_box_box_1) { } BOOST_AUTO_TEST_CASE(distance_box_box_2) { - CollisionGeometryPtr_t s1(new hpp::fcl::Box(6, 10, 2)); - CollisionGeometryPtr_t s2(new hpp::fcl::Box(2, 2, 2)); + CollisionGeometryPtr_t s1(new coal::Box(6, 10, 2)); + CollisionGeometryPtr_t s2(new coal::Box(2, 2, 2)); static double pi = M_PI; - Transform3f tf1; - Transform3f tf2( - hpp::fcl::makeQuat(cos(pi / 8), sin(pi / 8) / sqrt(3), - sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3)), - Vec3f(0, 0, 10)); + Transform3s tf1; + Transform3s tf2(coal::makeQuat(cos(pi / 8), sin(pi / 8) / sqrt(3), + sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3)), + Vec3s(0, 0, 10)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -117,7 +116,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2) { DistanceRequest distanceRequest(true, true, 0, 0); DistanceResult distanceResult; - hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult); + coal::distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied transformations on two boxes" << std::endl; std::cerr << " T1 = " << tf1.getTranslation() << std::endl @@ -128,8 +127,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2) { << ", p2 = " << distanceResult.nearest_points[1] << ", distance = " << distanceResult.min_distance << std::endl; - const Vec3f& p1 = distanceResult.nearest_points[0]; - const Vec3f& p2 = distanceResult.nearest_points[1]; + const Vec3s& p1 = distanceResult.nearest_points[0]; + const Vec3s& p2 = distanceResult.nearest_points[1]; double distance = -1.62123444 + 10 - 1; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); @@ -142,13 +141,13 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2) { } BOOST_AUTO_TEST_CASE(distance_box_box_3) { - CollisionGeometryPtr_t s1(new hpp::fcl::Box(1, 1, 1)); - CollisionGeometryPtr_t s2(new hpp::fcl::Box(1, 1, 1)); + CollisionGeometryPtr_t s1(new coal::Box(1, 1, 1)); + CollisionGeometryPtr_t s2(new coal::Box(1, 1, 1)); static double pi = M_PI; - Transform3f tf1(hpp::fcl::makeQuat(cos(pi / 8), 0, 0, sin(pi / 8)), - Vec3f(-2, 1, .5)); - Transform3f tf2(hpp::fcl::makeQuat(cos(pi / 8), 0, sin(pi / 8), 0), - Vec3f(2, .5, .5)); + Transform3s tf1(coal::makeQuat(cos(pi / 8), 0, 0, sin(pi / 8)), + Vec3s(-2, 1, .5)); + Transform3s tf2(coal::makeQuat(cos(pi / 8), 0, sin(pi / 8), 0), + Vec3s(2, .5, .5)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -157,7 +156,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) { DistanceRequest distanceRequest(true, 0, 0); DistanceResult distanceResult; - hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult); + coal::distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied transformations on two boxes" << std::endl; std::cerr << " T1 = " << tf1.getTranslation() << std::endl @@ -168,13 +167,13 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) { << ", p2 = " << distanceResult.nearest_points[1] << ", distance = " << distanceResult.min_distance << std::endl; - const Vec3f& p1 = distanceResult.nearest_points[0]; - const Vec3f& p2 = distanceResult.nearest_points[1]; + const Vec3s& p1 = distanceResult.nearest_points[0]; + const Vec3s& p2 = distanceResult.nearest_points[1]; double distance = 4 - sqrt(2); BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); - const Vec3f p1Ref(sqrt(2) / 2 - 2, 1, .5); - const Vec3f p2Ref(2 - sqrt(2) / 2, 1, .5); + const Vec3s p1Ref(sqrt(2) / 2 - 2, 1, .5); + const Vec3s p2Ref(2 - sqrt(2) / 2, 1, .5); BOOST_CHECK_CLOSE(p1[0], p1Ref[0], 1e-4); BOOST_CHECK_CLOSE(p1[1], p1Ref[1], 1e-4); BOOST_CHECK_CLOSE(p1[2], p1Ref[2], 1e-4); @@ -183,16 +182,16 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) { BOOST_CHECK_CLOSE(p2[2], p2Ref[2], 1e-4); // Apply the same global transform to both objects and recompute - Transform3f tf3(hpp::fcl::makeQuat(0.435952844074, -0.718287018243, - 0.310622451066, 0.444435113443), - Vec3f(4, 5, 6)); + Transform3s tf3(coal::makeQuat(0.435952844074, -0.718287018243, + 0.310622451066, 0.444435113443), + Vec3s(4, 5, 6)); tf1 = tf3 * tf1; tf2 = tf3 * tf2; o1 = CollisionObject(s1, tf1); o2 = CollisionObject(s2, tf2); distanceResult.clear(); - hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult); + coal::distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied transformations on two boxes" << std::endl; std::cerr << " T1 = " << tf1.getTranslation() << std::endl @@ -205,8 +204,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) { BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); - const Vec3f p1Moved = tf3.transform(p1Ref); - const Vec3f p2Moved = tf3.transform(p2Ref); + const Vec3s p1Moved = tf3.transform(p1Ref); + const Vec3s p2Moved = tf3.transform(p2Ref); BOOST_CHECK_CLOSE(p1[0], p1Moved[0], 1e-4); BOOST_CHECK_CLOSE(p1[1], p1Moved[1], 1e-4); BOOST_CHECK_CLOSE(p1[2], p1Moved[2], 1e-4); @@ -216,38 +215,38 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) { } BOOST_AUTO_TEST_CASE(distance_box_box_4) { - hpp::fcl::Box s1(1, 1, 1); - hpp::fcl::Box s2(1, 1, 1); + coal::Box s1(1, 1, 1); + coal::Box s2(1, 1, 1); // Enable computation of nearest points DistanceRequest distanceRequest(true, true, 0, 0); DistanceResult distanceResult; double distance; - Transform3f tf1(Vec3f(2, 0, 0)); - Transform3f tf2; - hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); + Transform3s tf1(Vec3s(2, 0, 0)); + Transform3s tf2; + coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = 1.; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); - tf1.setTranslation(Vec3f(1.01, 0, 0)); + tf1.setTranslation(Vec3s(1.01, 0, 0)); distanceResult.clear(); - hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); + coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = 0.01; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); - tf1.setTranslation(Vec3f(0.99, 0, 0)); + tf1.setTranslation(Vec3s(0.99, 0, 0)); distanceResult.clear(); - hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); + coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = -0.01; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); - tf1.setTranslation(Vec3f(0, 0, 0)); + tf1.setTranslation(Vec3s(0, 0, 0)); distanceResult.clear(); - hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); + coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = -1; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); diff --git a/test/broadphase.cpp b/test/broadphase.cpp index 65b96cff6..a28b85636 100644 --- a/test/broadphase.cpp +++ b/test/broadphase.cpp @@ -35,13 +35,13 @@ /** \author Jia Pan */ -#define BOOST_TEST_MODULE FCL_BROADPHASE +#define BOOST_TEST_MODULE COAL_BROADPHASE #include -#include -#include -#include -#include +#include "coal/config.hh" +#include "coal/broadphase/broadphase.h" +#include "coal/shape/geometric_shape_to_BVH_model.h" +#include "coal/math/transform.h" #include "utility.h" #if USE_GOOGLEHASH @@ -54,8 +54,8 @@ #include #include -using namespace hpp::fcl; -using namespace hpp::fcl::detail; +using namespace coal; +using namespace coal::detail; /// @brief Generate environment with 3 * n objects for self distance, so we try /// to make sure none of them collide with each other. @@ -75,7 +75,7 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size, void broad_phase_self_distance_test(double env_scale, std::size_t env_size, bool use_mesh = false); -FCL_REAL DELTA = 0.01; +CoalScalar DELTA = 0.01; #if USE_GOOGLEHASH template @@ -146,9 +146,9 @@ void generateSelfDistanceEnvironments(std::vector& env, double env_scale, std::size_t n) { int n_edge = static_cast(std::floor(std::pow(n, 1 / 3.0))); - FCL_REAL step_size = env_scale * 2 / n_edge; - FCL_REAL delta_size = step_size * 0.05; - FCL_REAL single_size = step_size - 2 * delta_size; + CoalScalar step_size = env_scale * 2 / n_edge; + CoalScalar delta_size = step_size * 0.05; + CoalScalar single_size = step_size - 2 * delta_size; int i = 0; for (; i < n_edge * n_edge * n_edge / 4; ++i) { @@ -159,7 +159,7 @@ void generateSelfDistanceEnvironments(std::vector& env, Box* box = new Box(single_size, single_size, single_size); env.push_back(new CollisionObject( shared_ptr(box), - Transform3f(Vec3f( + Transform3s(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -174,7 +174,7 @@ void generateSelfDistanceEnvironments(std::vector& env, Sphere* sphere = new Sphere(single_size / 2); env.push_back(new CollisionObject( shared_ptr(sphere), - Transform3f(Vec3f( + Transform3s(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -189,7 +189,7 @@ void generateSelfDistanceEnvironments(std::vector& env, Cylinder* cylinder = new Cylinder(single_size / 2, single_size); env.push_back(new CollisionObject( shared_ptr(cylinder), - Transform3f(Vec3f( + Transform3s(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -204,7 +204,7 @@ void generateSelfDistanceEnvironments(std::vector& env, Cone* cone = new Cone(single_size / 2, single_size); env.push_back(new CollisionObject( shared_ptr(cone), - Transform3f(Vec3f( + Transform3s(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -216,9 +216,9 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n) { int n_edge = static_cast(std::floor(std::pow(n, 1 / 3.0))); - FCL_REAL step_size = env_scale * 2 / n_edge; - FCL_REAL delta_size = step_size * 0.05; - FCL_REAL single_size = step_size - 2 * delta_size; + CoalScalar step_size = env_scale * 2 / n_edge; + CoalScalar delta_size = step_size * 0.05; + CoalScalar single_size = step_size - 2 * delta_size; int i = 0; for (; i < n_edge * n_edge * n_edge / 4; ++i) { @@ -228,10 +228,10 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, Box box(single_size, single_size, single_size); BVHModel* model = new BVHModel(); - generateBVHModel(*model, box, Transform3f()); + generateBVHModel(*model, box, Transform3s()); env.push_back(new CollisionObject( shared_ptr(model), - Transform3f(Vec3f( + Transform3s(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -245,10 +245,10 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, Sphere sphere(single_size / 2); BVHModel* model = new BVHModel(); - generateBVHModel(*model, sphere, Transform3f(), 16, 16); + generateBVHModel(*model, sphere, Transform3s(), 16, 16); env.push_back(new CollisionObject( shared_ptr(model), - Transform3f(Vec3f( + Transform3s(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -262,10 +262,10 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, Cylinder cylinder(single_size / 2, single_size); BVHModel* model = new BVHModel(); - generateBVHModel(*model, cylinder, Transform3f(), 16, 16); + generateBVHModel(*model, cylinder, Transform3s(), 16, 16); env.push_back(new CollisionObject( shared_ptr(model), - Transform3f(Vec3f( + Transform3s(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -279,10 +279,10 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, Cone cone(single_size / 2, single_size); BVHModel* model = new BVHModel(); - generateBVHModel(*model, cone, Transform3f(), 16, 16); + generateBVHModel(*model, cone, Transform3s(), 16, 16); env.push_back(new CollisionObject( shared_ptr(model), - Transform3f(Vec3f( + Transform3s(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -308,11 +308,12 @@ void broad_phase_self_distance_test(double env_scale, std::size_t env_size, managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); - Vec3f lower_limit, upper_limit; + Vec3s lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 5, - (upper_limit[1] - lower_limit[1]) / 5), - (upper_limit[2] - lower_limit[2]) / 5); + CoalScalar cell_size = + std::min(std::min((upper_limit[0] - lower_limit[0]) / 5, + (upper_limit[1] - lower_limit[1]) / 5), + (upper_limit[2] - lower_limit[2]) / 5); // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, // lower_limit, upper_limit)); managers.push_back(new SpatialHashingCollisionManager< @@ -457,9 +458,9 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size, managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); - Vec3f lower_limit, upper_limit; + Vec3s lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - FCL_REAL cell_size = + CoalScalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2]) / 20); @@ -564,7 +565,7 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size, std::cout << "distance time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) { - FCL_REAL tmp = 0; + CoalScalar tmp = 0; for (size_t j = 2; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } diff --git a/test/broadphase_collision_1.cpp b/test/broadphase_collision_1.cpp index 4798e66d0..4afeda0c5 100644 --- a/test/broadphase_collision_1.cpp +++ b/test/broadphase_collision_1.cpp @@ -35,19 +35,19 @@ */ /** @author Jia Pan */ -#define BOOST_TEST_MODULE BROADPHASE_COLLISION_1 +#define BOOST_TEST_MODULE COAL_BROADPHASE_COLLISION_1 #include -#include "hpp/fcl/broadphase/broadphase_bruteforce.h" -#include "hpp/fcl/broadphase/broadphase_spatialhash.h" -#include "hpp/fcl/broadphase/broadphase_SaP.h" -#include "hpp/fcl/broadphase/broadphase_SSaP.h" -#include "hpp/fcl/broadphase/broadphase_interval_tree.h" -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" -#include "hpp/fcl/broadphase/default_broadphase_callbacks.h" -#include "hpp/fcl/broadphase/detail/sparse_hash_table.h" -#include "hpp/fcl/broadphase/detail/spatial_hash.h" +#include "coal/broadphase/broadphase_bruteforce.h" +#include "coal/broadphase/broadphase_spatialhash.h" +#include "coal/broadphase/broadphase_SaP.h" +#include "coal/broadphase/broadphase_SSaP.h" +#include "coal/broadphase/broadphase_interval_tree.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "coal/broadphase/default_broadphase_callbacks.h" +#include "coal/broadphase/detail/sparse_hash_table.h" +#include "coal/broadphase/detail/spatial_hash.h" #include "utility.h" #include @@ -61,15 +61,17 @@ #include #include -using namespace hpp::fcl; +using namespace coal; /// @brief make sure if broadphase algorithms doesn't check twice for the same /// collision object pair -void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, +void broad_phase_duplicate_check_test(CoalScalar env_scale, + std::size_t env_size, bool verbose = false); /// @brief test for broad phase update -void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size, +void broad_phase_update_collision_test(CoalScalar env_scale, + std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, @@ -197,8 +199,8 @@ struct CollisionFunctionForUniquenessChecking : CollisionCallBackBase { }; //============================================================================== -void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, - bool verbose) { +void broad_phase_duplicate_check_test(CoalScalar env_scale, + std::size_t env_size, bool verbose) { std::vector ts; std::vector timers; @@ -210,9 +212,9 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, managers.push_back(new SSaPCollisionManager()); managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); - Vec3f lower_limit, upper_limit; + Vec3s lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - FCL_REAL cell_size = + CoalScalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2]) / 20); @@ -264,30 +266,30 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, } // update the environment - FCL_REAL delta_angle_max = - 10 / 360.0 * 2 * boost::math::constants::pi(); - FCL_REAL delta_trans_max = 0.01 * env_scale; + CoalScalar delta_angle_max = + 10 / 360.0 * 2 * boost::math::constants::pi(); + CoalScalar delta_trans_max = 0.01 * env_scale; for (size_t i = 0; i < env.size(); ++i) { - FCL_REAL rand_angle_x = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_x = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - FCL_REAL rand_angle_y = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_y = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - FCL_REAL rand_angle_z = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_z = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - - Matrix3f dR(Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) * - Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) * - Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ())); - Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z); - - Matrix3f R = env[i]->getRotation(); - Vec3f T = env[i]->getTranslation(); + CoalScalar rand_angle_x = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max; + CoalScalar rand_trans_x = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max; + CoalScalar rand_angle_y = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max; + CoalScalar rand_trans_y = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max; + CoalScalar rand_angle_z = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max; + CoalScalar rand_trans_z = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max; + + Matrix3s dR(Eigen::AngleAxisd(rand_angle_x, Vec3s::UnitX()) * + Eigen::AngleAxisd(rand_angle_y, Vec3s::UnitY()) * + Eigen::AngleAxisd(rand_angle_z, Vec3s::UnitZ())); + Vec3s dT(rand_trans_x, rand_trans_y, rand_trans_z); + + Matrix3s R = env[i]->getRotation(); + Vec3s T = env[i]->getTranslation(); env[i]->setTransform(dR * R, dR * T + dT); env[i]->computeAABB(); } @@ -340,7 +342,7 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, std::cout << "collision time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) { - FCL_REAL tmp = 0; + CoalScalar tmp = 0; for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } @@ -353,7 +355,8 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, std::cout << std::endl; } -void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size, +void broad_phase_update_collision_test(CoalScalar env_scale, + std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh) { @@ -380,9 +383,9 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size, managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); - Vec3f lower_limit, upper_limit; + Vec3s lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - FCL_REAL cell_size = + CoalScalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2]) / 20); @@ -436,30 +439,30 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size, } // update the environment - FCL_REAL delta_angle_max = - 10 / 360.0 * 2 * boost::math::constants::pi(); - FCL_REAL delta_trans_max = 0.01 * env_scale; + CoalScalar delta_angle_max = + 10 / 360.0 * 2 * boost::math::constants::pi(); + CoalScalar delta_trans_max = 0.01 * env_scale; for (size_t i = 0; i < env.size(); ++i) { - FCL_REAL rand_angle_x = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_x = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - FCL_REAL rand_angle_y = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_y = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - FCL_REAL rand_angle_z = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_z = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - - Matrix3f dR(Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) * - Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) * - Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ())); - Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z); - - Matrix3f R = env[i]->getRotation(); - Vec3f T = env[i]->getTranslation(); + CoalScalar rand_angle_x = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max; + CoalScalar rand_trans_x = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max; + CoalScalar rand_angle_y = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max; + CoalScalar rand_trans_y = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max; + CoalScalar rand_angle_z = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max; + CoalScalar rand_trans_z = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max; + + Matrix3s dR(Eigen::AngleAxisd(rand_angle_x, Vec3s::UnitX()) * + Eigen::AngleAxisd(rand_angle_y, Vec3s::UnitY()) * + Eigen::AngleAxisd(rand_angle_z, Vec3s::UnitZ())); + Vec3s dT(rand_trans_x, rand_trans_y, rand_trans_z); + + Matrix3s R = env[i]->getRotation(); + Vec3s T = env[i]->getTranslation(); env[i]->setTransform(dR * R, dR * T + dT); env[i]->computeAABB(); } @@ -578,7 +581,7 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size, std::cout << "collision time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) { - FCL_REAL tmp = 0; + CoalScalar tmp = 0; for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } diff --git a/test/broadphase_collision_2.cpp b/test/broadphase_collision_2.cpp index a6a667465..fe5465829 100644 --- a/test/broadphase_collision_2.cpp +++ b/test/broadphase_collision_2.cpp @@ -35,19 +35,19 @@ /** @author Jia Pan */ -#define BOOST_TEST_MODULE BROADPHASE_COLLISION_2 +#define BOOST_TEST_MODULE COAL_BROADPHASE_COLLISION_2 #include -#include "hpp/fcl/broadphase/broadphase_bruteforce.h" -#include "hpp/fcl/broadphase/broadphase_spatialhash.h" -#include "hpp/fcl/broadphase/broadphase_SaP.h" -#include "hpp/fcl/broadphase/broadphase_SSaP.h" -#include "hpp/fcl/broadphase/broadphase_interval_tree.h" -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" -#include "hpp/fcl/broadphase/default_broadphase_callbacks.h" -#include "hpp/fcl/broadphase/detail/sparse_hash_table.h" -#include "hpp/fcl/broadphase/detail/spatial_hash.h" +#include "coal/broadphase/broadphase_bruteforce.h" +#include "coal/broadphase/broadphase_spatialhash.h" +#include "coal/broadphase/broadphase_SaP.h" +#include "coal/broadphase/broadphase_SSaP.h" +#include "coal/broadphase/broadphase_interval_tree.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "coal/broadphase/default_broadphase_callbacks.h" +#include "coal/broadphase/detail/sparse_hash_table.h" +#include "coal/broadphase/detail/spatial_hash.h" #include "utility.h" #if USE_GOOGLEHASH @@ -59,10 +59,10 @@ #include #include -using namespace hpp::fcl; +using namespace coal; /// @brief test for broad phase collision and self collision -void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size, +void broad_phase_collision_test(CoalScalar env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, bool use_mesh = false); @@ -182,7 +182,7 @@ BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_exhaustive) { #endif } -void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size, +void broad_phase_collision_test(CoalScalar env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh) { @@ -208,11 +208,11 @@ void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size, managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); - Vec3f lower_limit, upper_limit; + Vec3s lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - // FCL_REAL ncell_per_axis = std::pow((S)env_size / 10, 1 / 3.0); - FCL_REAL ncell_per_axis = 20; - FCL_REAL cell_size = + // CoalScalar ncell_per_axis = std::pow((S)env_size / 10, 1 / 3.0); + CoalScalar ncell_per_axis = 20; + CoalScalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / ncell_per_axis, (upper_limit[1] - lower_limit[1]) / ncell_per_axis), (upper_limit[2] - lower_limit[2]) / ncell_per_axis); @@ -365,7 +365,7 @@ void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size, std::cout << "collision time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) { - FCL_REAL tmp = 0; + CoalScalar tmp = 0; for (size_t j = 3; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } diff --git a/test/broadphase_dynamic_AABB_tree.cpp b/test/broadphase_dynamic_AABB_tree.cpp index c16b09d15..f13c73e80 100644 --- a/test/broadphase_dynamic_AABB_tree.cpp +++ b/test/broadphase_dynamic_AABB_tree.cpp @@ -36,17 +36,17 @@ /** Tests the dynamic axis-aligned bounding box tree.*/ -#include -#include - -#define BOOST_TEST_MODULE BROADPHASE_DYNAMIC_AABB_TREE +#define BOOST_TEST_MODULE COAL_BROADPHASE_DYNAMIC_AABB_TREE #include -// #include "hpp/fcl/data_types.h" -#include "hpp/fcl/shape/geometric_shapes.h" -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" +// #include "coal/data_types.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree.h" + +#include +#include -using namespace hpp::fcl; +using namespace coal; // Pack the data for callback function. struct CallBackData { @@ -58,16 +58,16 @@ struct CallBackData { // the dynamic tree against the `data`. We assume that the first two // parameters are always objects[0] and objects[1] in two possible orders, // so we can safely ignore the second parameter. We do not use the last -// FCL_REAL& parameter, which specifies the distance beyond which the +// CoalScalar& parameter, which specifies the distance beyond which the // pair of objects will be skipped. struct DistanceCallBackDerived : DistanceCallBackBase { - bool distance(CollisionObject* o1, CollisionObject* o2, FCL_REAL& dist) { + bool distance(CollisionObject* o1, CollisionObject* o2, CoalScalar& dist) { return distance_callback(o1, o2, &data, dist); } bool distance_callback(CollisionObject* a, CollisionObject*, - void* callback_data, FCL_REAL&) { + void* callback_data, CoalScalar&) { // Unpack the data. CallBackData* data = static_cast(callback_data); const std::vector& objects = *(data->objects); diff --git a/test/bvh_models.cpp b/test/bvh_models.cpp index 10c54a39a..74a946621 100644 --- a/test/bvh_models.cpp +++ b/test/bvh_models.cpp @@ -35,32 +35,32 @@ /** \author Jeongseok Lee */ -#define BOOST_TEST_MODULE FCL_BVH_MODELS +#define BOOST_TEST_MODULE COAL_BVH_MODELS #include #include #include "fcl_resources/config.h" -#include -#include -#include -#include -#include -#include -#include -#include +#include "coal/collision.h" +#include "coal/BVH/BVH_model.h" +#include "coal/BVH/BVH_utility.h" +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/shape/geometric_shape_to_BVH_model.h" +#include "coal/mesh_loader/assimp.h" +#include "coal/mesh_loader/loader.h" #include "utility.h" #include -using namespace hpp::fcl; +using namespace coal; template void testBVHModelPointCloud() { - Box box(Vec3f::Ones()); + Box box(Vec3s::Ones()); double a = box.halfSide[0]; double b = box.halfSide[1]; double c = box.halfSide[2]; - std::vector points(8); + std::vector points(8); points[0] << a, -b, c; points[1] << a, b, c; points[2] << -a, b, c; @@ -114,7 +114,7 @@ void testBVHModelPointCloud() { return; } - Matrixx3f all_points((Eigen::DenseIndex)points.size(), 3); + MatrixX3s all_points((Eigen::DenseIndex)points.size(), 3); for (size_t k = 0; k < points.size(); ++k) all_points.row((Eigen::DenseIndex)k) = points[k].transpose(); @@ -139,13 +139,13 @@ void testBVHModelPointCloud() { template void testBVHModelTriangles() { shared_ptr > model(new BVHModel); - Box box(Vec3f::Ones()); - AABB aabb(Vec3f(-1, 0, -1), Vec3f(1, 1, 1)); + Box box(Vec3s::Ones()); + AABB aabb(Vec3s(-1, 0, -1), Vec3s(1, 1, 1)); double a = box.halfSide[0]; double b = box.halfSide[1]; double c = box.halfSide[2]; - std::vector points(8); + std::vector points(8); std::vector tri_indices(12); points[0] << a, -b, c; points[1] << a, b, c; @@ -190,22 +190,22 @@ void testBVHModelTriangles() { BOOST_CHECK_EQUAL(model->num_tris, 12); BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED); - Transform3f pose; + Transform3s pose; shared_ptr > cropped(BVHExtract(*model, pose, aabb)); BOOST_REQUIRE(cropped); BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED); BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6); BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2); - pose.setTranslation(Vec3f(0, 1, 0)); + pose.setTranslation(Vec3s(0, 1, 0)); cropped.reset(BVHExtract(*model, pose, aabb)); BOOST_REQUIRE(cropped); BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED); BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6); BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2); - pose.setTranslation(Vec3f(0, 0, 0)); - FCL_REAL sqrt2_2 = std::sqrt(2) / 2; + pose.setTranslation(Vec3s(0, 0, 0)); + CoalScalar sqrt2_2 = std::sqrt(2) / 2; pose.setQuatRotation(Quatf(sqrt2_2, sqrt2_2, 0, 0)); cropped.reset(BVHExtract(*model, pose, aabb)); BOOST_REQUIRE(cropped); @@ -213,13 +213,13 @@ void testBVHModelTriangles() { BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6); BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2); - pose.setTranslation(-Vec3f(1, 1, 1)); + pose.setTranslation(-Vec3s(1, 1, 1)); pose.setQuatRotation(Quatf::Identity()); cropped.reset(BVHExtract(*model, pose, aabb)); BOOST_CHECK(!cropped); - aabb = AABB(Vec3f(-0.1, -0.1, -0.1), Vec3f(0.1, 0.1, 0.1)); - pose.setTranslation(Vec3f(-0.5, -0.5, 0)); + aabb = AABB(Vec3s(-0.1, -0.1, -0.1), Vec3s(0.1, 0.1, 0.1)); + pose.setTranslation(Vec3s(-0.5, -0.5, 0)); cropped.reset(BVHExtract(*model, pose, aabb)); BOOST_REQUIRE(cropped); BOOST_CHECK_EQUAL(cropped->num_tris, 2); @@ -229,12 +229,12 @@ void testBVHModelTriangles() { template void testBVHModelSubModel() { shared_ptr > model(new BVHModel); - Box box(Vec3f::Ones()); + Box box(Vec3s::Ones()); double a = box.halfSide[0]; double b = box.halfSide[1]; double c = box.halfSide[2]; - std::vector points(8); + std::vector points(8); std::vector tri_indices(12); points[0] << a, -b, c; points[1] << a, b, c; @@ -304,7 +304,7 @@ void testLoadPolyhedron() { typedef shared_ptr PolyhedronPtr_t; PolyhedronPtr_t P1(new Polyhedron_t), P2; - Vec3f scale; + Vec3s scale; scale.setConstant(1); loadPolyhedronFromResource(env, scale, P1); @@ -331,11 +331,11 @@ void testLoadGerardBauzil() { typedef shared_ptr PolyhedronPtr_t; PolyhedronPtr_t P1(new Polyhedron_t), P2; - Vec3f scale; + Vec3s scale; scale.setConstant(1); loadPolyhedronFromResource(env, scale, P1); CollisionGeometryPtr_t cylinder(new Cylinder(.27, .27)); - Transform3f pos(Vec3f(-1.33, 1.36, .14)); + Transform3s pos(Vec3s(-1.33, 1.36, .14)); CollisionObject obj(cylinder, pos); CollisionObject stairs(P1); @@ -373,10 +373,10 @@ BOOST_AUTO_TEST_CASE(load_illformated_mesh) { } BOOST_AUTO_TEST_CASE(test_convex) { - Box* box_ptr = new hpp::fcl::Box(1, 1, 1); + Box* box_ptr = new coal::Box(1, 1, 1); CollisionGeometryPtr_t b1(box_ptr); BVHModel box_bvh_model = BVHModel(); - generateBVHModel(box_bvh_model, *box_ptr, Transform3f()); + generateBVHModel(box_bvh_model, *box_ptr, Transform3s()); box_bvh_model.buildConvexRepresentation(false); box_bvh_model.convex->computeLocalAABB(); diff --git a/test/capsule_box_1.cpp b/test/capsule_box_1.cpp index e86e5328f..30b882b02 100644 --- a/test/capsule_box_1.cpp +++ b/test/capsule_box_1.cpp @@ -34,42 +34,42 @@ /** \author Florent Lamiraux */ -#define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES +#define BOOST_TEST_MODULE COAL_GEOMETRIC_SHAPES #include #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) #include -#include -#include -#include -#include -#include +#include "coal/distance.h" +#include "coal/math/transform.h" +#include "coal/collision.h" +#include "coal/collision_object.h" +#include "coal/shape/geometric_shapes.h" #include "utility.h" BOOST_AUTO_TEST_CASE(distance_capsule_box) { - using hpp::fcl::CollisionGeometryPtr_t; + using coal::CollisionGeometryPtr_t; // Capsule of radius 2 and of height 4 - CollisionGeometryPtr_t capsuleGeometry(new hpp::fcl::Capsule(2., 4.)); + CollisionGeometryPtr_t capsuleGeometry(new coal::Capsule(2., 4.)); // Box of size 1 by 2 by 4 - CollisionGeometryPtr_t boxGeometry(new hpp::fcl::Box(1., 2., 4.)); + CollisionGeometryPtr_t boxGeometry(new coal::Box(1., 2., 4.)); // Enable computation of nearest points - hpp::fcl::DistanceRequest distanceRequest(true, 0, 0); - hpp::fcl::DistanceResult distanceResult; + coal::DistanceRequest distanceRequest(true, 0, 0); + coal::DistanceResult distanceResult; - hpp::fcl::Transform3f tf1(hpp::fcl::Vec3f(3., 0, 0)); - hpp::fcl::Transform3f tf2; - hpp::fcl::CollisionObject capsule(capsuleGeometry, tf1); - hpp::fcl::CollisionObject box(boxGeometry, tf2); + coal::Transform3s tf1(coal::Vec3s(3., 0, 0)); + coal::Transform3s tf2; + coal::CollisionObject capsule(capsuleGeometry, tf1); + coal::CollisionObject box(boxGeometry, tf2); // test distance - hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult); + coal::distance(&capsule, &box, distanceRequest, distanceResult); // Nearest point on capsule - hpp::fcl::Vec3f o1(distanceResult.nearest_points[0]); + coal::Vec3s o1(distanceResult.nearest_points[0]); // Nearest point on box - hpp::fcl::Vec3f o2(distanceResult.nearest_points[1]); + coal::Vec3s o2(distanceResult.nearest_points[1]); BOOST_CHECK_CLOSE(distanceResult.min_distance, 0.5, 1e-1); BOOST_CHECK_CLOSE(o1[0], 1.0, 1e-1); CHECK_CLOSE_TO_0(o1[1], 1e-1); @@ -77,12 +77,12 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) { CHECK_CLOSE_TO_0(o2[1], 1e-1); // Move capsule above box - tf1 = hpp::fcl::Transform3f(hpp::fcl::Vec3f(0., 0., 8.)); + tf1 = coal::Transform3s(coal::Vec3s(0., 0., 8.)); capsule.setTransform(tf1); // test distance distanceResult.clear(); - hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult); + coal::distance(&capsule, &box, distanceRequest, distanceResult); o1 = distanceResult.nearest_points[0]; o2 = distanceResult.nearest_points[1]; @@ -96,13 +96,13 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) { BOOST_CHECK_CLOSE(o2[2], 2.0, 1e-1); // Rotate capsule around y axis by pi/2 and move it behind box - tf1.setTranslation(hpp::fcl::Vec3f(-10., 0., 0.)); - tf1.setQuatRotation(hpp::fcl::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0)); + tf1.setTranslation(coal::Vec3s(-10., 0., 0.)); + tf1.setQuatRotation(coal::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0)); capsule.setTransform(tf1); // test distance distanceResult.clear(); - hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult); + coal::distance(&capsule, &box, distanceRequest, distanceResult); o1 = distanceResult.nearest_points[0]; o2 = distanceResult.nearest_points[1]; diff --git a/test/capsule_box_2.cpp b/test/capsule_box_2.cpp index add71efd6..bdf84d592 100644 --- a/test/capsule_box_2.cpp +++ b/test/capsule_box_2.cpp @@ -34,7 +34,7 @@ /** \author Florent Lamiraux */ -#define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES +#define BOOST_TEST_MODULE COAL_GEOMETRIC_SHAPES #include #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) @@ -42,36 +42,35 @@ #include "utility.h" #include -#include -#include -#include -#include -#include +#include "coal/distance.h" +#include "coal/math/transform.h" +#include "coal/collision.h" +#include "coal/collision_object.h" +#include "coal/shape/geometric_shapes.h" BOOST_AUTO_TEST_CASE(distance_capsule_box) { - typedef hpp::fcl::shared_ptr - CollisionGeometryPtr_t; + typedef coal::shared_ptr CollisionGeometryPtr_t; // Capsule of radius 2 and of height 4 - CollisionGeometryPtr_t capsuleGeometry(new hpp::fcl::Capsule(2., 4.)); + CollisionGeometryPtr_t capsuleGeometry(new coal::Capsule(2., 4.)); // Box of size 1 by 2 by 4 - CollisionGeometryPtr_t boxGeometry(new hpp::fcl::Box(1., 2., 4.)); + CollisionGeometryPtr_t boxGeometry(new coal::Box(1., 2., 4.)); // Enable computation of nearest points - hpp::fcl::DistanceRequest distanceRequest(true, 0, 0); - hpp::fcl::DistanceResult distanceResult; + coal::DistanceRequest distanceRequest(true, 0, 0); + coal::DistanceResult distanceResult; // Rotate capsule around y axis by pi/2 and move it behind box - hpp::fcl::Transform3f tf1(hpp::fcl::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), - hpp::fcl::Vec3f(-10., 0.8, 1.5)); - hpp::fcl::Transform3f tf2; - hpp::fcl::CollisionObject capsule(capsuleGeometry, tf1); - hpp::fcl::CollisionObject box(boxGeometry, tf2); + coal::Transform3s tf1(coal::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), + coal::Vec3s(-10., 0.8, 1.5)); + coal::Transform3s tf2; + coal::CollisionObject capsule(capsuleGeometry, tf1); + coal::CollisionObject box(boxGeometry, tf2); // test distance distanceResult.clear(); - hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult); - hpp::fcl::Vec3f o1 = distanceResult.nearest_points[0]; - hpp::fcl::Vec3f o2 = distanceResult.nearest_points[1]; + coal::distance(&capsule, &box, distanceRequest, distanceResult); + coal::Vec3s o1 = distanceResult.nearest_points[0]; + coal::Vec3s o2 = distanceResult.nearest_points[1]; BOOST_CHECK_CLOSE(distanceResult.min_distance, 5.5, 1e-2); BOOST_CHECK_CLOSE(o1[0], -6, 1e-2); diff --git a/test/capsule_capsule.cpp b/test/capsule_capsule.cpp index 25d95455d..b3f3e5304 100644 --- a/test/capsule_capsule.cpp +++ b/test/capsule_capsule.cpp @@ -35,23 +35,23 @@ /** \author Karsten Knese */ -#define BOOST_TEST_MODULE FCL_CAPSULE_CAPSULE +#define BOOST_TEST_MODULE COAL_CAPSULE_CAPSULE #include #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) #include #include -#include -#include -#include -#include -#include -#include +#include "coal/distance.h" +#include "coal/collision.h" +#include "coal/math/transform.h" +#include "coal/collision.h" +#include "coal/collision_object.h" +#include "coal/shape/geometric_shapes.h" #include "utility.h" -using namespace hpp::fcl; +using namespace coal; BOOST_AUTO_TEST_CASE(collision_capsule_capsule_trivial) { const double radius = 1.; @@ -68,8 +68,8 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_trivial) { int num_tests = 1e6; #endif - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; for (int i = 0; i < num_tests; ++i) { Eigen::Vector3d p1 = Eigen::Vector3d::Random() * (2. * radius); @@ -121,8 +121,8 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) { int num_tests = 1e6; #endif - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; Eigen::Vector3d p1 = Eigen::Vector3d::Zero(); Eigen::Vector3d p2_no_collision = @@ -180,9 +180,9 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) { p2_no_collision = Eigen::Vector3d(0., 0., 2 * (length / 2. + radius) + 1e-3); - Transform3f geom1_placement(Eigen::Matrix3d::Identity(), + Transform3s geom1_placement(Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero()); - Transform3f geom2_placement(Eigen::Matrix3d::Identity(), p2_no_collision); + Transform3s geom2_placement(Eigen::Matrix3d::Identity(), p2_no_collision); for (int i = 0; i < num_tests; ++i) { Eigen::Matrix3d rot = @@ -190,9 +190,9 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) { .toRotationMatrix(); Eigen::Vector3d trans = Eigen::Vector3d::Random(); - Transform3f displacement(rot, trans); - Transform3f tf1 = displacement * geom1_placement; - Transform3f tf2 = displacement * geom2_placement; + Transform3s displacement(rot, trans); + Transform3s tf1 = displacement * geom1_placement; + Transform3s tf2 = displacement * geom2_placement; CollisionObject capsule_o1(c1, tf1); CollisionObject capsule_o2(c2, tf2); @@ -218,9 +218,9 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) { .toRotationMatrix(); Eigen::Vector3d trans = Eigen::Vector3d::Random(); - Transform3f displacement(rot, trans); - Transform3f tf1 = displacement * geom1_placement; - Transform3f tf2 = displacement * geom2_placement; + Transform3s displacement(rot, trans); + Transform3s tf1 = displacement * geom1_placement; + Transform3s tf2 = displacement * geom2_placement; CollisionObject capsule_o1(c1, tf1); CollisionObject capsule_o2(c2, tf2); @@ -240,8 +240,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin) { CollisionGeometryPtr_t s1(new Capsule(5, 10)); CollisionGeometryPtr_t s2(new Capsule(5, 10)); - Transform3f tf1; - Transform3f tf2(Vec3f(20.1, 0, 0)); + Transform3s tf1; + Transform3s tf2(Vec3s(20.1, 0, 0)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -266,8 +266,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY) { CollisionGeometryPtr_t s1(new Capsule(5, 10)); CollisionGeometryPtr_t s2(new Capsule(5, 10)); - Transform3f tf1; - Transform3f tf2(Vec3f(20, 20, 0)); + Transform3s tf1; + Transform3s tf2(Vec3s(20, 20, 0)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -285,7 +285,7 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY) { << ", p2 = " << distanceResult.nearest_points[1] << ", distance = " << distanceResult.min_distance << std::endl; - FCL_REAL expected = sqrt(800) - 10; + CoalScalar expected = sqrt(800) - 10; BOOST_CHECK_CLOSE(distanceResult.min_distance, expected, 1e-6); } @@ -293,8 +293,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ) { CollisionGeometryPtr_t s1(new Capsule(5, 10)); CollisionGeometryPtr_t s2(new Capsule(5, 10)); - Transform3f tf1; - Transform3f tf2(Vec3f(0, 0, 20.1)); + Transform3s tf1; + Transform3s tf2(Vec3s(0, 0, 20.1)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -319,8 +319,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2) { CollisionGeometryPtr_t s1(new Capsule(5, 10)); CollisionGeometryPtr_t s2(new Capsule(5, 10)); - Transform3f tf1; - Transform3f tf2(makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), Vec3f(0, 0, 25.1)); + Transform3s tf1; + Transform3s tf2(makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), Vec3s(0, 0, 25.1)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -343,8 +343,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2) { << std::endl << "distance = " << distanceResult.min_distance << std::endl; - const Vec3f& p1 = distanceResult.nearest_points[0]; - const Vec3f& p2 = distanceResult.nearest_points[1]; + const Vec3s& p1 = distanceResult.nearest_points[0]; + const Vec3s& p2 = distanceResult.nearest_points[1]; BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6); CHECK_CLOSE_TO_0(p1[0], 1e-4); diff --git a/test/collision.cpp b/test/collision.cpp index 5b70aae4b..70c8d2810 100644 --- a/test/collision.cpp +++ b/test/collision.cpp @@ -37,60 +37,60 @@ #include -#define BOOST_TEST_MODULE FCL_COLLISION +#define BOOST_TEST_MODULE COAL_COLLISION #include #include #include -#include +#include "coal/fwd.hh" -HPP_FCL_COMPILER_DIAGNOSTIC_PUSH -HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS +COAL_COMPILER_DIAGNOSTIC_PUSH +COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS -#include +#include "coal/collision.h" -HPP_FCL_COMPILER_DIAGNOSTIC_POP +COAL_COMPILER_DIAGNOSTIC_POP -#include -#include -#include -#include +#include "coal/BV/BV.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/mesh_loader/assimp.h" -#include -#include +#include "coal/internal/traversal_node_bvhs.h" +#include "coal/internal/traversal_node_setup.h" #include "../src/collision_node.h" -#include +#include "coal/internal/BV_splitter.h" -#include +#include "coal/timings.h" #include "utility.h" #include "fcl_resources/config.h" -using namespace hpp::fcl; +using namespace coal; namespace utf = boost::unit_test::framework; int num_max_contacts = (std::numeric_limits::max)(); BOOST_AUTO_TEST_CASE(OBB_Box_test) { - FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; - std::vector rotate_transform; + CoalScalar r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; + std::vector rotate_transform; generateRandomTransforms(r_extents, rotate_transform, 1); AABB aabb1; - aabb1.min_ = Vec3f(-600, -600, -600); - aabb1.max_ = Vec3f(600, 600, 600); + aabb1.min_ = Vec3s(-600, -600, -600); + aabb1.max_ = Vec3s(600, 600, 600); OBB obb1; convertBV(aabb1, rotate_transform[0], obb1); Box box1; - Transform3f box1_tf; + Transform3s box1_tf; constructBox(aabb1, rotate_transform[0], box1, box1_tf); - FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; + CoalScalar extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; - std::vector transforms; + std::vector transforms; generateRandomTransforms(extents, transforms, n); for (std::size_t i = 0; i < transforms.size(); ++i) { @@ -102,7 +102,7 @@ BOOST_AUTO_TEST_CASE(OBB_Box_test) { convertBV(aabb, transforms[i], obb2); Box box2; - Transform3f box2_tf; + Transform3s box2_tf; constructBox(aabb, transforms[i], box2, box2_tf); GJKSolver solver; @@ -123,28 +123,28 @@ BOOST_AUTO_TEST_CASE(OBB_Box_test) { } BOOST_AUTO_TEST_CASE(OBB_shape_test) { - FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; - std::vector rotate_transform; + CoalScalar r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; + std::vector rotate_transform; generateRandomTransforms(r_extents, rotate_transform, 1); AABB aabb1; - aabb1.min_ = Vec3f(-600, -600, -600); - aabb1.max_ = Vec3f(600, 600, 600); + aabb1.min_ = Vec3s(-600, -600, -600); + aabb1.max_ = Vec3s(600, 600, 600); OBB obb1; convertBV(aabb1, rotate_transform[0], obb1); Box box1; - Transform3f box1_tf; + Transform3s box1_tf; constructBox(aabb1, rotate_transform[0], box1, box1_tf); - FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; + CoalScalar extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; - std::vector transforms; + std::vector transforms; generateRandomTransforms(extents, transforms, n); for (std::size_t i = 0; i < transforms.size(); ++i) { - FCL_REAL len = (aabb1.max_[0] - aabb1.min_[0]) * 0.5; + CoalScalar len = (aabb1.max_[0] - aabb1.min_[0]) * 0.5; OBB obb2; GJKSolver solver; @@ -206,18 +206,18 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test) { } BOOST_AUTO_TEST_CASE(OBB_AABB_test) { - FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; + CoalScalar extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; - std::vector transforms; + std::vector transforms; generateRandomTransforms(extents, transforms, n); AABB aabb1; - aabb1.min_ = Vec3f(-600, -600, -600); - aabb1.max_ = Vec3f(600, 600, 600); + aabb1.min_ = Vec3s(-600, -600, -600); + aabb1.max_ = Vec3s(600, 600, 600); OBB obb1; - convertBV(aabb1, Transform3f(), obb1); + convertBV(aabb1, Transform3s(), obb1); for (std::size_t i = 0; i < transforms.size(); ++i) { AABB aabb; @@ -227,7 +227,7 @@ BOOST_AUTO_TEST_CASE(OBB_AABB_test) { AABB aabb2 = translate(aabb, transforms[i].getTranslation()); OBB obb2; - convertBV(aabb, Transform3f(transforms[i].getTranslation()), obb2); + convertBV(aabb, Transform3s(transforms[i].getTranslation()), obb2); bool overlap_aabb = aabb1.overlap(aabb2); bool overlap_obb = obb1.overlap(obb2); @@ -308,11 +308,11 @@ struct traits, Oriented, recursive> : base_traits { enum { IS_IMPLEMENTED = false }; }; -HPP_FCL_COMPILER_DIAGNOSTIC_PUSH -HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS +COAL_COMPILER_DIAGNOSTIC_PUSH +COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS struct mesh_mesh_run_test { - mesh_mesh_run_test(const std::vector& _transforms, + mesh_mesh_run_test(const std::vector& _transforms, const CollisionRequest _request) : transforms(_transforms), request(_request), @@ -321,7 +321,7 @@ struct mesh_mesh_run_test { isInit(false), indent(0) {} - const std::vector& transforms; + const std::vector& transforms; const CollisionRequest request; bool enable_statistics, benchmark; std::vector contacts; @@ -330,7 +330,7 @@ struct mesh_mesh_run_test { int indent; - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP const char* getindent() { assert(indent < 9); @@ -347,7 +347,7 @@ struct mesh_mesh_run_test { } template - void query(const std::vector& transforms, + void query(const std::vector& transforms, SplitMethodType splitMethod, const CollisionRequest request, std::vector& contacts) { BENCHMARK_HEADER("BV"); @@ -369,13 +369,13 @@ struct mesh_mesh_run_test { model1->bv_splitter.reset(new BVSplitter(splitMethod)); model2->bv_splitter.reset(new BVSplitter(splitMethod)); - loadPolyhedronFromResource(TEST_RESOURCES_DIR "/env.obj", Vec3f::Ones(), + loadPolyhedronFromResource(TEST_RESOURCES_DIR "/env.obj", Vec3s::Ones(), model1); - loadPolyhedronFromResource(TEST_RESOURCES_DIR "/rob.obj", Vec3f::Ones(), + loadPolyhedronFromResource(TEST_RESOURCES_DIR "/rob.obj", Vec3s::Ones(), model2); Timer timer(false); - const Transform3f tf2; + const Transform3s tf2; const std::size_t N = transforms.size(); contacts.resize(3 * N); @@ -385,7 +385,7 @@ struct mesh_mesh_run_test { ++indent; for (std::size_t i = 0; i < transforms.size(); ++i) { - const Transform3f& tf1 = transforms[i]; + const Transform3s& tf1 = transforms[i]; timer.start(); CollisionResult local_result; @@ -429,7 +429,7 @@ struct mesh_mesh_run_test { ++indent; for (std::size_t i = 0; i < transforms.size(); ++i) { - const Transform3f tf1 = transforms[i]; + const Transform3s tf1 = transforms[i]; timer.start(); CollisionResult local_result; @@ -438,9 +438,9 @@ struct mesh_mesh_run_test { node.enable_statistics = enable_statistics; BVH_t* model1_tmp = new BVH_t(*model1); - Transform3f tf1_tmp = tf1; + Transform3s tf1_tmp = tf1; BVH_t* model2_tmp = new BVH_t(*model2); - Transform3f tf2_tmp = tf2; + Transform3s tf2_tmp = tf2; bool success = initialize(node, *model1_tmp, tf1_tmp, *model2_tmp, tf2_tmp, local_result, true, true); @@ -482,7 +482,7 @@ struct mesh_mesh_run_test { for (std::size_t i = 0; i < transforms.size(); ++i) { timer.start(); - const Transform3f tf1 = transforms[i]; + const Transform3s tf1 = transforms[i]; CollisionResult local_result; MeshCollisionTraversalNode node(request); @@ -623,8 +623,8 @@ struct mesh_mesh_run_test { // calls function collide with identity for both object poses, // BOOST_AUTO_TEST_CASE(mesh_mesh) { - std::vector transforms; - FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; + std::vector transforms; + CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; #ifndef NDEBUG // if debug mode std::size_t n = 1; #else @@ -641,8 +641,8 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) { << transforms[i].getQuatRotation().coeffs().format(f)); } - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS // Request all contacts and check that all methods give the same result. mesh_mesh_run_test runner( @@ -650,12 +650,12 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) { runner.enable_statistics = true; boost::mpl::for_each >(runner); - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP } BOOST_AUTO_TEST_CASE(mesh_mesh_benchmark) { - std::vector transforms; - FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; + std::vector transforms; + CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; #ifndef NDEBUG std::size_t n = 0; #else diff --git a/test/collision_node_asserts.cpp b/test/collision_node_asserts.cpp index f5b6de08b..698d1d5b4 100644 --- a/test/collision_node_asserts.cpp +++ b/test/collision_node_asserts.cpp @@ -1,24 +1,24 @@ -#define BOOST_TEST_MODULE FCL_COLLISION_NODE_ASSERT +#define BOOST_TEST_MODULE COAL_COLLISION_NODE_ASSERT #include #include -#include -#include +#include "coal/BVH/BVH_model.h" +#include "coal/collision.h" -using namespace hpp::fcl; +using namespace coal; -constexpr FCL_REAL pi = boost::math::constants::pi(); +constexpr CoalScalar pi = boost::math::constants::pi(); double DegToRad(const double& deg) { static double degToRad = pi / 180.; return deg * degToRad; } -std::vector dirs{Vec3f::UnitZ(), -Vec3f::UnitZ(), Vec3f::UnitY(), - -Vec3f::UnitY(), Vec3f::UnitX(), -Vec3f::UnitX()}; +std::vector dirs{Vec3s::UnitZ(), -Vec3s::UnitZ(), Vec3s::UnitY(), + -Vec3s::UnitY(), Vec3s::UnitX(), -Vec3s::UnitX()}; BOOST_AUTO_TEST_CASE(TestTriangles) { - std::vector triVertices{Vec3f(1, 0, 0), Vec3f(1, 1, 0), - Vec3f(0, 1, 0)}; + std::vector triVertices{Vec3s(1, 0, 0), Vec3s(1, 1, 0), + Vec3s(0, 1, 0)}; std::vector triangle{{0, 1, 2}}; BVHModel tri1{}; @@ -36,19 +36,19 @@ BOOST_AUTO_TEST_CASE(TestTriangles) { ComputeCollision compute(&tri1, &tri2); - Transform3f tri1Tf{}; - Transform3f tri2Tf{}; + Transform3s tri1Tf{}; + Transform3s tri2Tf{}; /// check some angles for two triangles for (int i = 0; i < 360; i += 30) { for (int j = 0; j < 180; j += 30) { for (int k = 0; k < 180; k += 30) { tri1Tf.setQuatRotation( - Eigen::AngleAxis(0., Vec3f::UnitZ()) * - Eigen::AngleAxis(DegToRad(k), Vec3f::UnitY())); + Eigen::AngleAxis(0., Vec3s::UnitZ()) * + Eigen::AngleAxis(DegToRad(k), Vec3s::UnitY())); tri2Tf.setQuatRotation( - Eigen::AngleAxis(DegToRad(i), Vec3f::UnitZ()) * - Eigen::AngleAxis(DegToRad(j), Vec3f::UnitY())); + Eigen::AngleAxis(DegToRad(i), Vec3s::UnitZ()) * + Eigen::AngleAxis(DegToRad(j), Vec3s::UnitY())); CollisionResult result; /// assertion: src/collision_node.cpp:58 diff --git a/test/contact_patch.cpp b/test/contact_patch.cpp index 4bebaaea9..a78b84f44 100644 --- a/test/contact_patch.cpp +++ b/test/contact_patch.cpp @@ -34,72 +34,72 @@ /** \author Louis Montaut */ -#define BOOST_TEST_MODULE FCL_CONTACT_PATCH +#define BOOST_TEST_MODULE COAL_CONTACT_PATCH #include -#include +#include "coal/contact_patch.h" #include "utility.h" -using namespace hpp::fcl; +using namespace coal; BOOST_AUTO_TEST_CASE(box_box_no_collision) { - const FCL_REAL halfside = 0.5; + const CoalScalar halfside = 0.5; const Box box1(2 * halfside, 2 * halfside, 2 * halfside); const Box box2(2 * halfside, 2 * halfside, 2 * halfside); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to separate the shapes - const FCL_REAL offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, 2 * halfside + offset)); + const CoalScalar offset = 0.001; + tf2.setTranslation(Vec3s(0, 0, 2 * halfside + offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&box1, tf1, &box2, tf2, col_req, col_res); + coal::collide(&box1, tf1, &box2, tf2, col_req, col_res); BOOST_CHECK(!col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res(patch_req); - hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, - patch_res); + coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 0); } BOOST_AUTO_TEST_CASE(box_sphere) { - const FCL_REAL halfside = 0.5; + const CoalScalar halfside = 0.5; const Box box(2 * halfside, 2 * halfside, 2 * halfside); const Sphere sphere(halfside); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to have a collision - const FCL_REAL offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset)); + const CoalScalar offset = 0.001; + tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&box, tf1, &sphere, tf2, col_req, col_res); + coal::collide(&box, tf1, &sphere, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res(patch_req); - hpp::fcl::computeContactPatch(&box, tf1, &sphere, tf2, col_res, patch_req, - patch_res); + coal::computeContactPatch(&box, tf1, &sphere, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(contact_patch.size() == 1); - const FCL_REAL tol = 1e-8; + const CoalScalar tol = 1e-8; EIGEN_VECTOR_IS_APPROX(contact_patch.getPoint(0), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.tf.translation(), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.getNormal(), contact.normal, tol); @@ -109,40 +109,40 @@ BOOST_AUTO_TEST_CASE(box_sphere) { } BOOST_AUTO_TEST_CASE(box_box) { - const FCL_REAL halfside = 0.5; + const CoalScalar halfside = 0.5; const Box box1(2 * halfside, 2 * halfside, 2 * halfside); const Box box2(2 * halfside, 2 * halfside, 2 * halfside); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to have a collision - const FCL_REAL offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset)); + const CoalScalar offset = 0.001; + tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&box1, tf1, &box2, tf2, col_req, col_res); + coal::collide(&box1, tf1, &box2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res1(patch_req); ContactPatchResult patch_res2(patch_req); - hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, - patch_res1); - hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, - patch_res2); + coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, + patch_res1); + coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, + patch_res2); BOOST_CHECK(patch_res1.numContactPatches() == 1); BOOST_CHECK(patch_res2.numContactPatches() == 1); if (patch_res1.numContactPatches() > 0 && patch_res2.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; - EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3f(0, 0, 1), tol); + const CoalScalar tol = 1e-6; + EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3s(0, 0, 1), tol); const size_t expected_size = 4; ContactPatch expected(expected_size); @@ -150,11 +150,11 @@ BOOST_AUTO_TEST_CASE(box_box) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - const std::array corners = { - Vec3f(halfside, halfside, halfside), - Vec3f(halfside, -halfside, halfside), - Vec3f(-halfside, -halfside, halfside), - Vec3f(-halfside, halfside, halfside), + const std::array corners = { + Vec3s(halfside, halfside, halfside), + Vec3s(halfside, -halfside, halfside), + Vec3s(-halfside, -halfside, halfside), + Vec3s(-halfside, halfside, halfside), }; for (size_t i = 0; i < expected_size; ++i) { expected.addPoint(corners[i] + @@ -168,40 +168,40 @@ BOOST_AUTO_TEST_CASE(box_box) { BOOST_AUTO_TEST_CASE(halfspace_box) { const Halfspace hspace(0, 0, 1, 0); - const FCL_REAL halfside = 0.5; + const CoalScalar halfside = 0.5; const Box box(2 * halfside, 2 * halfside, 2 * halfside); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to have a collision - const FCL_REAL offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, halfside - offset)); + const CoalScalar offset = 0.001; + tf2.setTranslation(Vec3s(0, 0, halfside - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&hspace, tf1, &box, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &box, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res1(patch_req); ContactPatchResult patch_res2(patch_req); - hpp::fcl::computeContactPatch(&hspace, tf1, &box, tf2, col_res, patch_req, - patch_res1); - hpp::fcl::computeContactPatch(&hspace, tf1, &box, tf2, col_res, patch_req, - patch_res2); + coal::computeContactPatch(&hspace, tf1, &box, tf2, col_res, patch_req, + patch_res1); + coal::computeContactPatch(&hspace, tf1, &box, tf2, col_res, patch_req, + patch_res2); BOOST_CHECK(patch_res1.numContactPatches() == 1); BOOST_CHECK(patch_res2.numContactPatches() == 1); if (patch_res1.numContactPatches() > 0 && patch_res2.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); - EIGEN_VECTOR_IS_APPROX(hspace.n, Vec3f(0, 0, 1), tol); + EIGEN_VECTOR_IS_APPROX(hspace.n, Vec3s(0, 0, 1), tol); const size_t expected_size = 4; ContactPatch expected(expected_size); @@ -209,11 +209,11 @@ BOOST_AUTO_TEST_CASE(halfspace_box) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - const std::array corners = { - tf2.transform(Vec3f(halfside, halfside, -halfside)), - tf2.transform(Vec3f(halfside, -halfside, -halfside)), - tf2.transform(Vec3f(-halfside, -halfside, -halfside)), - tf2.transform(Vec3f(-halfside, halfside, -halfside)), + const std::array corners = { + tf2.transform(Vec3s(halfside, halfside, -halfside)), + tf2.transform(Vec3s(halfside, -halfside, -halfside)), + tf2.transform(Vec3s(-halfside, -halfside, -halfside)), + tf2.transform(Vec3s(-halfside, halfside, -halfside)), }; for (size_t i = 0; i < expected_size; ++i) { expected.addPoint(corners[i] - @@ -227,34 +227,34 @@ BOOST_AUTO_TEST_CASE(halfspace_box) { BOOST_AUTO_TEST_CASE(halfspace_capsule) { const Halfspace hspace(0, 0, 1, 0); - const FCL_REAL radius = 0.25; - const FCL_REAL height = 1.; + const CoalScalar radius = 0.25; + const CoalScalar height = 1.; const Capsule capsule(radius, height); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to have a collision - const FCL_REAL offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, height / 2 - offset)); + const CoalScalar offset = 0.001; + tf2.setTranslation(Vec3s(0, 0, height / 2 - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; BOOST_CHECK(patch_req.getNumSamplesCurvedShapes() == ContactPatch::default_preallocated_size); ContactPatchResult patch_res(patch_req); - hpp::fcl::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, - patch_res); + coal::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); const size_t expected_size = 1; @@ -263,7 +263,7 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - const Vec3f capsule_end(0, 0, -capsule.halfLength); + const Vec3s capsule_end(0, 0, -capsule.halfLength); expected.addPoint(tf2.transform(capsule_end)); const ContactPatch& contact_patch = patch_res.getContactPatch(0); @@ -277,15 +277,15 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { tf2.rotation().col(1) << 0, 1, 0; tf2.rotation().col(2) << 0, 0, -1; col_res.clear(); - hpp::fcl::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, - patch_res); + coal::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); const size_t expected_size = 1; @@ -294,7 +294,7 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - const Vec3f capsule_end(0, 0, capsule.halfLength); + const Vec3s capsule_end(0, 0, capsule.halfLength); expected.addPoint(tf2.transform(capsule_end)); const ContactPatch& contact_patch = patch_res.getContactPatch(0); @@ -309,15 +309,15 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { tf2.rotation().col(2) << -1, 0, 0; tf2.translation() << 0, 0, capsule.radius - offset; col_res.clear(); - hpp::fcl::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, - patch_res); + coal::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); const size_t expected_size = 2; @@ -326,8 +326,8 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - const Vec3f p1(-capsule.radius, 0, capsule.halfLength); - const Vec3f p2(-capsule.radius, 0, -capsule.halfLength); + const Vec3s p1(-capsule.radius, 0, capsule.halfLength); + const Vec3s p2(-capsule.radius, 0, -capsule.halfLength); expected.addPoint(tf2.transform(p1)); expected.addPoint(tf2.transform(p2)); @@ -339,34 +339,34 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { BOOST_AUTO_TEST_CASE(halfspace_cone) { const Halfspace hspace(0, 0, 1, 0); - const FCL_REAL radius = 0.25; - const FCL_REAL height = 1.; + const CoalScalar radius = 0.25; + const CoalScalar height = 1.; const Cone cone(radius, height); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to have a collision - const FCL_REAL offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, height / 2 - offset)); + const CoalScalar offset = 0.001; + tf2.setTranslation(Vec3s(0, 0, height / 2 - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&hspace, tf1, &cone, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &cone, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; BOOST_CHECK(patch_req.getNumSamplesCurvedShapes() == ContactPatch::default_preallocated_size); ContactPatchResult patch_res(patch_req); - hpp::fcl::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, - patch_res); + coal::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); const size_t expected_size = ContactPatch::default_preallocated_size; @@ -375,12 +375,12 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - std::array points; - const FCL_REAL angle_increment = - 2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(6)); + std::array points; + const CoalScalar angle_increment = + 2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(6)); for (size_t i = 0; i < ContactPatch::default_preallocated_size; ++i) { - const FCL_REAL theta = (FCL_REAL)(i)*angle_increment; - Vec3f point_on_cone_base(std::cos(theta) * cone.radius, + const CoalScalar theta = (CoalScalar)(i)*angle_increment; + Vec3s point_on_cone_base(std::cos(theta) * cone.radius, std::sin(theta) * cone.radius, -cone.halfLength); expected.addPoint(tf2.transform(point_on_cone_base)); } @@ -396,17 +396,17 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { tf2.rotation().col(1) << 0, 1, 0; tf2.rotation().col(2) << 0, 0, -1; col_res.clear(); - hpp::fcl::collide(&hspace, tf1, &cone, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &cone, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, - patch_res); + coal::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(contact_patch.size() == 1); - const FCL_REAL tol = 1e-8; + const CoalScalar tol = 1e-8; EIGEN_VECTOR_IS_APPROX(contact_patch.getPoint(0), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.tf.translation(), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.getNormal(), contact.normal, tol); @@ -419,7 +419,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - const Vec3f cone_tip(0, 0, cone.halfLength); + const Vec3s cone_tip(0, 0, cone.halfLength); expected.addPoint(tf2.transform(cone_tip)); BOOST_CHECK(contact_patch.isSame(expected, tol)); @@ -432,17 +432,17 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { tf2.rotation().col(2) << -1, 0, 0; tf2.translation() << 0, 0, cone.radius - offset; col_res.clear(); - hpp::fcl::collide(&hspace, tf1, &cone, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &cone, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, - patch_res); + coal::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(contact_patch.size() == 1); - const FCL_REAL tol = 1e-8; + const CoalScalar tol = 1e-8; EIGEN_VECTOR_IS_APPROX(contact_patch.getPoint(0), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.tf.translation(), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.getNormal(), contact.normal, tol); @@ -455,7 +455,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - const Vec3f point_on_circle_basis(-cone.radius, 0, -cone.halfLength); + const Vec3s point_on_circle_basis(-cone.radius, 0, -cone.halfLength); expected.addPoint(tf2.transform(point_on_circle_basis)); BOOST_CHECK(contact_patch.isSame(expected, tol)); @@ -464,38 +464,38 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { BOOST_AUTO_TEST_CASE(halfspace_cylinder) { const Halfspace hspace(0, 0, 1, 0); - const FCL_REAL radius = 0.25; - const FCL_REAL height = 1.; + const CoalScalar radius = 0.25; + const CoalScalar height = 1.; const Cylinder cylinder(radius, height); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to have a collision - const FCL_REAL offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, height / 2 - offset)); + const CoalScalar offset = 0.001; + tf2.setTranslation(Vec3s(0, 0, height / 2 - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); if (col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const size_t expected_size = ContactPatch::default_preallocated_size; - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - std::array points; - const FCL_REAL angle_increment = - 2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(6)); + std::array points; + const CoalScalar angle_increment = + 2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(6)); for (size_t i = 0; i < ContactPatch::default_preallocated_size; ++i) { - const FCL_REAL theta = (FCL_REAL)(i)*angle_increment; - Vec3f point_on_cone_base(std::cos(theta) * cylinder.radius, + const CoalScalar theta = (CoalScalar)(i)*angle_increment; + Vec3s point_on_cone_base(std::cos(theta) * cylinder.radius, std::sin(theta) * cylinder.radius, -cylinder.halfLength); expected.addPoint(tf2.transform(point_on_cone_base)); @@ -505,8 +505,8 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) { BOOST_CHECK(patch_req.getNumSamplesCurvedShapes() == ContactPatch::default_preallocated_size); ContactPatchResult patch_res(patch_req); - hpp::fcl::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { @@ -522,11 +522,11 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) { tf2.rotation().col(1) << 0, 1, 0; tf2.rotation().col(2) << 0, 0, -1; col_res.clear(); - hpp::fcl::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); @@ -544,17 +544,17 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) { tf2.translation() << 0, 0, cylinder.radius - offset; col_res.clear(); - hpp::fcl::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res(patch_req); - hpp::fcl::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (col_res.isCollision() && patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; const size_t expected_size = 2; ContactPatch expected(expected_size); @@ -563,9 +563,9 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) { expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; expected.addPoint( - tf2.transform(Vec3f(cylinder.radius, 0, cylinder.halfLength))); + tf2.transform(Vec3s(cylinder.radius, 0, cylinder.halfLength))); expected.addPoint( - tf2.transform(Vec3f(cylinder.radius, 0, -cylinder.halfLength))); + tf2.transform(Vec3s(cylinder.radius, 0, -cylinder.halfLength))); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(expected.isSame(contact_patch, tol)); @@ -573,40 +573,40 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) { } BOOST_AUTO_TEST_CASE(convex_convex) { - const FCL_REAL halfside = 0.5; + const CoalScalar halfside = 0.5; const Convex box1(buildBox(halfside, halfside, halfside)); const Convex box2(buildBox(halfside, halfside, halfside)); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to have a collision - const FCL_REAL offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset)); + const CoalScalar offset = 0.001; + tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&box1, tf1, &box2, tf2, col_req, col_res); + coal::collide(&box1, tf1, &box2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res1(patch_req); ContactPatchResult patch_res2(patch_req); - hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, - patch_res1); - hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, - patch_res2); + coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, + patch_res1); + coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, + patch_res2); BOOST_CHECK(patch_res1.numContactPatches() == 1); BOOST_CHECK(patch_res2.numContactPatches() == 1); if (patch_res1.numContactPatches() > 0 && patch_res2.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; - EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3f(0, 0, 1), tol); + const CoalScalar tol = 1e-6; + EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3s(0, 0, 1), tol); const size_t expected_size = 4; ContactPatch expected(expected_size); @@ -614,11 +614,11 @@ BOOST_AUTO_TEST_CASE(convex_convex) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - const std::array corners = { - Vec3f(halfside, halfside, halfside), - Vec3f(halfside, -halfside, halfside), - Vec3f(-halfside, -halfside, halfside), - Vec3f(-halfside, halfside, halfside), + const std::array corners = { + Vec3s(halfside, halfside, halfside), + Vec3s(halfside, -halfside, halfside), + Vec3s(-halfside, -halfside, halfside), + Vec3s(-halfside, halfside, halfside), }; for (size_t i = 0; i < expected_size; ++i) { expected.addPoint(corners[i] + @@ -635,11 +635,11 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { // Two tetrahedrons make contact on one of their edge. const size_t expected_size = 2; - const Vec3f expected_cp1(0, 0.5, 0); - const Vec3f expected_cp2(0, 1, 0); + const Vec3s expected_cp1(0, 0.5, 0); + const Vec3s expected_cp2(0, 1, 0); - const Transform3f tf1; // identity - const Transform3f tf2; // identity + const Transform3s tf1; // identity + const Transform3s tf2; // identity const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, @@ -650,22 +650,22 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { { // Case 1 - Face-Face contact - std::shared_ptr> pts1(new std::vector({ - Vec3f(-1, 0, 0), - Vec3f(0, 0, 0), - Vec3f(0, 1, 0), - Vec3f(-1, -1, -1), + std::shared_ptr> pts1(new std::vector({ + Vec3s(-1, 0, 0), + Vec3s(0, 0, 0), + Vec3s(0, 1, 0), + Vec3s(-1, -1, -1), })); std::shared_ptr> tris1( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), Triangle(0, 3, 1), Triangle(2, 1, 3)})); Convex tetra1(pts1, 4, tris1, 4); - std::shared_ptr> pts2(new std::vector({ - Vec3f(0, 0.5, 0), - Vec3f(0, 1.5, 0), - Vec3f(1, 0.5, 0), - Vec3f(1, 1, 1), + std::shared_ptr> pts2(new std::vector({ + Vec3s(0, 0.5, 0), + Vec3s(0, 1.5, 0), + Vec3s(1, 0.5, 0), + Vec3s(1, 1, 1), })); std::shared_ptr> tris2( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), @@ -673,16 +673,16 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { Convex tetra2(pts2, 4, tris2, 4); col_res.clear(); - hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; ContactPatch expected(expected_size); // GJK/EPA can return any normal which is in the dual cone @@ -701,22 +701,22 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { { // Case 2 - Face-Segment contact - std::shared_ptr> pts1(new std::vector({ - Vec3f(-1, 0, -0.2), - Vec3f(0, 0, 0), - Vec3f(0, 1, 0), - Vec3f(-1, -1, -1), + std::shared_ptr> pts1(new std::vector({ + Vec3s(-1, 0, -0.2), + Vec3s(0, 0, 0), + Vec3s(0, 1, 0), + Vec3s(-1, -1, -1), })); std::shared_ptr> tris1( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), Triangle(0, 3, 1), Triangle(2, 1, 3)})); Convex tetra1(pts1, 4, tris1, 4); - std::shared_ptr> pts2(new std::vector({ - Vec3f(0, 0.5, 0), - Vec3f(0, 1.5, 0), - Vec3f(1, 0.5, 0), - Vec3f(1, 1, 1), + std::shared_ptr> pts2(new std::vector({ + Vec3s(0, 0.5, 0), + Vec3s(0, 1.5, 0), + Vec3s(1, 0.5, 0), + Vec3s(1, 1, 1), })); std::shared_ptr> tris2( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), @@ -724,16 +724,16 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { Convex tetra2(pts2, 4, tris2, 4); col_res.clear(); - hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; ContactPatch expected(expected_size); expected.tf.rotation() = @@ -750,22 +750,22 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { { // Case 3 - Segment-Segment contact - std::shared_ptr> pts1(new std::vector({ - Vec3f(-1, 0, -0.2), - Vec3f(0, 0, 0), - Vec3f(0, 1, 0), - Vec3f(-1, -1, -1), + std::shared_ptr> pts1(new std::vector({ + Vec3s(-1, 0, -0.2), + Vec3s(0, 0, 0), + Vec3s(0, 1, 0), + Vec3s(-1, -1, -1), })); std::shared_ptr> tris1( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), Triangle(0, 3, 1), Triangle(2, 1, 3)})); Convex tetra1(pts1, 4, tris1, 4); - std::shared_ptr> pts2(new std::vector({ - Vec3f(0, 0.5, 0), - Vec3f(0, 1.5, 0), - Vec3f(1, 0.5, 0.5), - Vec3f(1, 1, 1), + std::shared_ptr> pts2(new std::vector({ + Vec3s(0, 0.5, 0), + Vec3s(0, 1.5, 0), + Vec3s(1, 0.5, 0.5), + Vec3s(1, 1, 1), })); std::shared_ptr> tris2( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), @@ -773,16 +773,16 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { Convex tetra2(pts2, 4, tris2, 4); col_res.clear(); - hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; ContactPatch expected(expected_size); expected.tf.rotation() = @@ -802,10 +802,10 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { // This case covers the vertex-vertex edge case of contact patches. // Two tetrahedrons make contact on one of their vertex. const size_t expected_size = 1; - const Vec3f expected_cp(0, 0, 0); + const Vec3s expected_cp(0, 0, 0); - const Transform3f tf1; // identity - const Transform3f tf2; // identity + const Transform3s tf1; // identity + const Transform3s tf2; // identity const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, @@ -816,22 +816,22 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { { // Case 1 - Face-Face contact - std::shared_ptr> pts1(new std::vector({ - Vec3f(-1, 0, 0), - Vec3f(0, 0, 0), - Vec3f(0, 1, 0), - Vec3f(-1, -1, -1), + std::shared_ptr> pts1(new std::vector({ + Vec3s(-1, 0, 0), + Vec3s(0, 0, 0), + Vec3s(0, 1, 0), + Vec3s(-1, -1, -1), })); std::shared_ptr> tris1( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), Triangle(0, 3, 1), Triangle(2, 1, 3)})); Convex tetra1(pts1, 4, tris1, 4); - std::shared_ptr> pts2(new std::vector({ - Vec3f(1, 0, 0), - Vec3f(0, 0, 0), - Vec3f(0, -1, 0), - Vec3f(1, 1, 1), + std::shared_ptr> pts2(new std::vector({ + Vec3s(1, 0, 0), + Vec3s(0, 0, 0), + Vec3s(0, -1, 0), + Vec3s(1, 1, 1), })); std::shared_ptr> tris2( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), @@ -839,16 +839,16 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { Convex tetra2(pts2, 4, tris2, 4); col_res.clear(); - hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; ContactPatch expected(expected_size); expected.tf.rotation() = @@ -864,22 +864,22 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { { // Case 2 - Segment-Face contact - std::shared_ptr> pts1(new std::vector({ - Vec3f(-1, 0, -0.5), - Vec3f(0, 0, 0), - Vec3f(0, 1, 0), - Vec3f(-1, -1, -1), + std::shared_ptr> pts1(new std::vector({ + Vec3s(-1, 0, -0.5), + Vec3s(0, 0, 0), + Vec3s(0, 1, 0), + Vec3s(-1, -1, -1), })); std::shared_ptr> tris1( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), Triangle(0, 3, 1), Triangle(2, 1, 3)})); Convex tetra1(pts1, 4, tris1, 4); - std::shared_ptr> pts2(new std::vector({ - Vec3f(1, 0, 0), - Vec3f(0, 0, 0), - Vec3f(0, -1, 0), - Vec3f(1, 1, 1), + std::shared_ptr> pts2(new std::vector({ + Vec3s(1, 0, 0), + Vec3s(0, 0, 0), + Vec3s(0, -1, 0), + Vec3s(1, 1, 1), })); std::shared_ptr> tris2( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), @@ -887,16 +887,16 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { Convex tetra2(pts2, 4, tris2, 4); col_res.clear(); - hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; ContactPatch expected(expected_size); expected.tf.rotation() = @@ -912,22 +912,22 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { { // Case 2 - Segment-Segment contact - std::shared_ptr> pts1(new std::vector({ - Vec3f(-1, 0, -0.2), - Vec3f(0, 0, 0), - Vec3f(0, 1, 0), - Vec3f(-1, -1, -1), + std::shared_ptr> pts1(new std::vector({ + Vec3s(-1, 0, -0.2), + Vec3s(0, 0, 0), + Vec3s(0, 1, 0), + Vec3s(-1, -1, -1), })); std::shared_ptr> tris1( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), Triangle(0, 3, 1), Triangle(2, 1, 3)})); Convex tetra1(pts1, 4, tris1, 4); - std::shared_ptr> pts2(new std::vector({ - Vec3f(1, 0, 0), - Vec3f(0, 0, 0), - Vec3f(0, -1, 0.5), - Vec3f(1, 1, 1), + std::shared_ptr> pts2(new std::vector({ + Vec3s(1, 0, 0), + Vec3s(0, 0, 0), + Vec3s(0, -1, 0.5), + Vec3s(1, 1, 1), })); std::shared_ptr> tris2( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), @@ -935,16 +935,16 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { Convex tetra2(pts2, 4, tris2, 4); col_res.clear(); - hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; ContactPatch expected(expected_size); expected.tf.rotation() = @@ -963,11 +963,11 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_face) { // This case covers the segment-face edge case of contact patches. // Two tetrahedrons make contact on one of their segment/face respectively. const size_t expected_size = 2; - const Vec3f expected_cp1(0, 0, 0); - const Vec3f expected_cp2(-0.5, 0.5, 0); + const Vec3s expected_cp1(0, 0, 0); + const Vec3s expected_cp2(-0.5, 0.5, 0); - const Transform3f tf1; // identity - const Transform3f tf2; // identity + const Transform3s tf1; // identity + const Transform3s tf2; // identity const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, @@ -977,22 +977,22 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_face) { ContactPatchResult patch_res(patch_req); { - std::shared_ptr> pts1(new std::vector({ - Vec3f(-1, 0, -0), - Vec3f(0, 0, 0), - Vec3f(0, 1, 0), - Vec3f(-1, -1, -1), + std::shared_ptr> pts1(new std::vector({ + Vec3s(-1, 0, -0), + Vec3s(0, 0, 0), + Vec3s(0, 1, 0), + Vec3s(-1, -1, -1), })); std::shared_ptr> tris1( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), Triangle(0, 3, 1), Triangle(2, 1, 3)})); Convex tetra1(pts1, 4, tris1, 4); - std::shared_ptr> pts2(new std::vector({ - Vec3f(-0.5, 0.5, 0), - Vec3f(0.5, -0.5, 0), - Vec3f(1, 0.5, 0.5), - Vec3f(1, 1, 1), + std::shared_ptr> pts2(new std::vector({ + Vec3s(-0.5, 0.5, 0), + Vec3s(0.5, -0.5, 0), + Vec3s(1, 0.5, 0.5), + Vec3s(1, 1, 1), })); std::shared_ptr> tris2( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), @@ -1000,16 +1000,16 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_face) { Convex tetra2(pts2, 4, tris2, 4); col_res.clear(); - hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; ContactPatch expected(expected_size); expected.tf.rotation() = diff --git a/test/convex.cpp b/test/convex.cpp index 744902b97..52363b7d8 100644 --- a/test/convex.cpp +++ b/test/convex.cpp @@ -34,19 +34,19 @@ /** \author Joseph Mirabel */ -#define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES +#define BOOST_TEST_MODULE COAL_GEOMETRIC_SHAPES #include -#include -#include -#include +#include "coal/shape/convex.h" +#include "coal/collision.h" +#include "coal/distance.h" #include "utility.h" -using namespace hpp::fcl; +using namespace coal; BOOST_AUTO_TEST_CASE(convex) { - FCL_REAL l = 1, w = 1, d = 1; + CoalScalar l = 1, w = 1, d = 1; Convex box(buildBox(l, w, d)); // Check neighbors @@ -88,8 +88,8 @@ BOOST_AUTO_TEST_CASE(convex) { template void compareShapeIntersection(const Sa& sa, const Sb& sb, - const Transform3f& tf1, const Transform3f& tf2, - FCL_REAL tol = 1e-9) { + const Transform3s& tf1, const Transform3s& tf2, + CoalScalar tol = 1e-9) { CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1); CollisionResult resA, resB; @@ -117,8 +117,8 @@ void compareShapeIntersection(const Sa& sa, const Sb& sb, } template -void compareShapeDistance(const Sa& sa, const Sb& sb, const Transform3f& tf1, - const Transform3f& tf2, FCL_REAL tol = 1e-9) { +void compareShapeDistance(const Sa& sa, const Sb& sb, const Transform3s& tf1, + const Transform3s& tf2, CoalScalar tol = 1e-9) { DistanceRequest request(true); DistanceResult resA, resB; @@ -149,19 +149,19 @@ void compareShapeDistance(const Sa& sa, const Sb& sb, const Transform3f& tf1, } BOOST_AUTO_TEST_CASE(compare_convex_box) { - FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10}; - FCL_REAL l = 1, w = 1, d = 1, eps = 1e-4; + CoalScalar extents[6] = {0, 0, 0, 10, 10, 10}; + CoalScalar l = 1, w = 1, d = 1, eps = 1e-4; Box box(l * 2, w * 2, d * 2); Convex convex_box(buildBox(l, w, d)); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - tf2.setTranslation(Vec3f(3, 0, 0)); + tf2.setTranslation(Vec3s(3, 0, 0)); compareShapeIntersection(box, convex_box, tf1, tf2, eps); compareShapeDistance(box, convex_box, tf1, tf2, eps); - tf2.setTranslation(Vec3f(0, 0, 0)); + tf2.setTranslation(Vec3s(0, 0, 0)); compareShapeIntersection(box, convex_box, tf1, tf2, eps); compareShapeDistance(box, convex_box, tf1, tf2, eps); @@ -172,10 +172,10 @@ BOOST_AUTO_TEST_CASE(compare_convex_box) { } } -#ifdef HPP_FCL_HAS_QHULL +#ifdef COAL_HAS_QHULL BOOST_AUTO_TEST_CASE(convex_hull_throw) { - std::shared_ptr> points( - new std::vector({Vec3f(1, 1, 1), Vec3f(0, 0, 0), Vec3f(1, 0, 0)})); + std::shared_ptr> points( + new std::vector({Vec3s(1, 1, 1), Vec3s(0, 0, 0), Vec3s(1, 0, 0)})); BOOST_CHECK_THROW(ConvexBase::convexHull(points, 0, false, NULL), std::invalid_argument); @@ -188,11 +188,11 @@ BOOST_AUTO_TEST_CASE(convex_hull_throw) { } BOOST_AUTO_TEST_CASE(convex_hull_quad) { - std::shared_ptr> points(new std::vector({ - Vec3f(1, 1, 1), - Vec3f(0, 0, 0), - Vec3f(1, 0, 0), - Vec3f(0, 0, 1), + std::shared_ptr> points(new std::vector({ + Vec3s(1, 1, 1), + Vec3s(0, 0, 0), + Vec3s(1, 0, 0), + Vec3s(0, 0, 1), })); ConvexBase* convexHull = ConvexBase::convexHull(points, 4, false, NULL); @@ -205,26 +205,26 @@ BOOST_AUTO_TEST_CASE(convex_hull_quad) { } BOOST_AUTO_TEST_CASE(convex_hull_box_like) { - std::shared_ptr> points(new std::vector({ - Vec3f(1, 1, 1), - Vec3f(1, 1, -1), - Vec3f(1, -1, 1), - Vec3f(1, -1, -1), - Vec3f(-1, 1, 1), - Vec3f(-1, 1, -1), - Vec3f(-1, -1, 1), - Vec3f(-1, -1, -1), - Vec3f(0, 0, 0), - Vec3f(0, 0, 0.99), + std::shared_ptr> points(new std::vector({ + Vec3s(1, 1, 1), + Vec3s(1, 1, -1), + Vec3s(1, -1, 1), + Vec3s(1, -1, -1), + Vec3s(-1, 1, 1), + Vec3s(-1, 1, -1), + Vec3s(-1, -1, 1), + Vec3s(-1, -1, -1), + Vec3s(0, 0, 0), + Vec3s(0, 0, 0.99), })); ConvexBase* convexHull = ConvexBase::convexHull(points, 9, false, NULL); BOOST_REQUIRE_EQUAL(8, convexHull->num_points); { - const std::vector& cvxhull_points = *(convexHull->points); + const std::vector& cvxhull_points = *(convexHull->points); for (size_t i = 0; i < 8; ++i) { - BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3f(1, 1, 1)); + BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3s(1, 1, 1)); BOOST_CHECK_EQUAL((*(convexHull->neighbors))[i].count(), 3); } } @@ -236,9 +236,9 @@ BOOST_AUTO_TEST_CASE(convex_hull_box_like) { BOOST_REQUIRE_EQUAL(8, convexHull->num_points); { - const std::vector& cvxhull_points = *(convexHull->points); + const std::vector& cvxhull_points = *(convexHull->points); for (size_t i = 0; i < 8; ++i) { - BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3f(1, 1, 1)); + BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3s(1, 1, 1)); BOOST_CHECK((*(convexHull->neighbors))[i].count() >= 3); BOOST_CHECK((*(convexHull->neighbors))[i].count() <= 6); } @@ -249,16 +249,16 @@ BOOST_AUTO_TEST_CASE(convex_hull_box_like) { BOOST_AUTO_TEST_CASE(convex_copy_constructor) { Convex* convexHullTriCopy; { - std::shared_ptr> points(new std::vector({ - Vec3f(1, 1, 1), - Vec3f(1, 1, -1), - Vec3f(1, -1, 1), - Vec3f(1, -1, -1), - Vec3f(-1, 1, 1), - Vec3f(-1, 1, -1), - Vec3f(-1, -1, 1), - Vec3f(-1, -1, -1), - Vec3f(0, 0, 0), + std::shared_ptr> points(new std::vector({ + Vec3s(1, 1, 1), + Vec3s(1, 1, -1), + Vec3s(1, -1, 1), + Vec3s(1, -1, -1), + Vec3s(-1, 1, 1), + Vec3s(-1, 1, -1), + Vec3s(-1, -1, 1), + Vec3s(-1, -1, -1), + Vec3s(0, 0, 0), })); Convex* convexHullTri = dynamic_cast*>( @@ -272,16 +272,16 @@ BOOST_AUTO_TEST_CASE(convex_copy_constructor) { } BOOST_AUTO_TEST_CASE(convex_clone) { - std::shared_ptr> points(new std::vector({ - Vec3f(1, 1, 1), - Vec3f(1, 1, -1), - Vec3f(1, -1, 1), - Vec3f(1, -1, -1), - Vec3f(-1, 1, 1), - Vec3f(-1, 1, -1), - Vec3f(-1, -1, 1), - Vec3f(-1, -1, -1), - Vec3f(0, 0, 0), + std::shared_ptr> points(new std::vector({ + Vec3s(1, 1, 1), + Vec3s(1, 1, -1), + Vec3s(1, -1, 1), + Vec3s(1, -1, -1), + Vec3s(-1, 1, 1), + Vec3s(-1, 1, -1), + Vec3s(-1, -1, 1), + Vec3s(-1, -1, -1), + Vec3s(0, 0, 0), })); Convex* convexHullTri = dynamic_cast*>( diff --git a/test/distance.cpp b/test/distance.cpp index ed365417a..acb892513 100644 --- a/test/distance.cpp +++ b/test/distance.cpp @@ -35,51 +35,51 @@ /** \author Jia Pan */ -#define BOOST_TEST_MODULE FCL_DISTANCE +#define BOOST_TEST_MODULE COAL_DISTANCE #include #include #include -#include -#include +#include "coal/internal/traversal_node_bvhs.h" +#include "coal/internal/traversal_node_setup.h" #include "../src/collision_node.h" -#include +#include "coal/internal/BV_splitter.h" #include "utility.h" #include "fcl_resources/config.h" -using namespace hpp::fcl; +using namespace coal; namespace utf = boost::unit_test::framework; bool verbose = false; -FCL_REAL DELTA = 0.001; +CoalScalar DELTA = 0.001; template -void distance_Test(const Transform3f& tf, const std::vector& vertices1, +void distance_Test(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, unsigned int qsize, DistanceRes& distance_result, bool verbose = true); -bool collide_Test_OBB(const Transform3f& tf, - const std::vector& vertices1, +bool collide_Test_OBB(const Transform3s& tf, + const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); template -void distance_Test_Oriented(const Transform3f& tf, - const std::vector& vertices1, +void distance_Test_Oriented(const Transform3s& tf, + const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, unsigned int qsize, DistanceRes& distance_result, bool verbose = true); -inline bool nearlyEqual(const Vec3f& a, const Vec3f& b) { +inline bool nearlyEqual(const Vec3s& a, const Vec3s& b) { if (fabs(a[0] - b[0]) > DELTA) return false; if (fabs(a[1] - b[1]) > DELTA) return false; if (fabs(a[2] - b[2]) > DELTA) return false; @@ -87,14 +87,14 @@ inline bool nearlyEqual(const Vec3f& a, const Vec3f& b) { } BOOST_AUTO_TEST_CASE(mesh_distance) { - std::vector p1, p2; + std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); - std::vector transforms; // t0 - FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; + std::vector transforms; // t0 + CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; #ifndef NDEBUG // if debug mode std::size_t n = 1; #else @@ -468,10 +468,10 @@ BOOST_AUTO_TEST_CASE(mesh_distance) { } template -void distance_Test_Oriented(const Transform3f& tf, - const std::vector& vertices1, +void distance_Test_Oriented(const Transform3s& tf, + const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, unsigned int qsize, DistanceRes& distance_result, bool verbose) { @@ -491,7 +491,7 @@ void distance_Test_Oriented(const Transform3f& tf, DistanceResult local_result; TraversalNode node; if (!initialize(node, (const BVHModel&)m1, tf, (const BVHModel&)m2, - Transform3f(), DistanceRequest(true), local_result)) + Transform3s(), DistanceRequest(true), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; @@ -499,8 +499,8 @@ void distance_Test_Oriented(const Transform3f& tf, distance(&node, NULL, qsize); // points are in local coordinate, to global coordinate - Vec3f p1 = local_result.nearest_points[0]; - Vec3f p2 = local_result.nearest_points[1]; + Vec3s p1 = local_result.nearest_points[0]; + Vec3s p2 = local_result.nearest_points[1]; distance_result.distance = local_result.min_distance; distance_result.p1 = p1; @@ -516,9 +516,9 @@ void distance_Test_Oriented(const Transform3f& tf, } template -void distance_Test(const Transform3f& tf, const std::vector& vertices1, +void distance_Test(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, unsigned int qsize, DistanceRes& distance_result, bool verbose) { @@ -535,7 +535,7 @@ void distance_Test(const Transform3f& tf, const std::vector& vertices1, m2.addSubModel(vertices2, triangles2); m2.endModel(); - Transform3f pose1(tf), pose2; + Transform3s pose1(tf), pose2; DistanceResult local_result; MeshDistanceTraversalNode node; @@ -565,10 +565,10 @@ void distance_Test(const Transform3f& tf, const std::vector& vertices1, } } -bool collide_Test_OBB(const Transform3f& tf, - const std::vector& vertices1, +bool collide_Test_OBB(const Transform3s& tf, + const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { BVHModel m1; @@ -588,7 +588,7 @@ bool collide_Test_OBB(const Transform3f& tf, CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1); MeshCollisionTraversalNodeOBB node(request); bool success(initialize(node, (const BVHModel&)m1, tf, - (const BVHModel&)m2, Transform3f(), + (const BVHModel&)m2, Transform3s(), local_result)); BOOST_REQUIRE(success); diff --git a/test/distance_lower_bound.cpp b/test/distance_lower_bound.cpp index d14825dd1..f4386597d 100644 --- a/test/distance_lower_bound.cpp +++ b/test/distance_lower_bound.cpp @@ -32,39 +32,39 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#define BOOST_TEST_MODULE FCL_DISTANCE_LOWER_BOUND +#define BOOST_TEST_MODULE COAL_DISTANCE_LOWER_BOUND #include #include -#include -#include -#include -#include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/data_types.h" +#include "coal/BV/OBBRSS.h" +#include "coal/BVH/BVH_model.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/collision.h" +#include "coal/distance.h" #include "utility.h" #include "fcl_resources/config.h" -using hpp::fcl::BVHModel; -using hpp::fcl::CollisionGeometryPtr_t; -using hpp::fcl::CollisionObject; -using hpp::fcl::CollisionRequest; -using hpp::fcl::CollisionResult; -using hpp::fcl::DistanceRequest; -using hpp::fcl::DistanceResult; -using hpp::fcl::FCL_REAL; -using hpp::fcl::OBBRSS; -using hpp::fcl::shared_ptr; -using hpp::fcl::Transform3f; -using hpp::fcl::Triangle; -using hpp::fcl::Vec3f; - -bool testDistanceLowerBound(const Transform3f& tf, +using coal::BVHModel; +using coal::CoalScalar; +using coal::CollisionGeometryPtr_t; +using coal::CollisionObject; +using coal::CollisionRequest; +using coal::CollisionResult; +using coal::DistanceRequest; +using coal::DistanceResult; +using coal::OBBRSS; +using coal::shared_ptr; +using coal::Transform3s; +using coal::Triangle; +using coal::Vec3s; + +bool testDistanceLowerBound(const Transform3s& tf, const CollisionGeometryPtr_t& m1, const CollisionGeometryPtr_t& m2, - FCL_REAL& distance) { - Transform3f pose1(tf), pose2; + CoalScalar& distance) { + Transform3s pose1(tf), pose2; CollisionRequest request; @@ -72,37 +72,37 @@ bool testDistanceLowerBound(const Transform3f& tf, CollisionObject co1(m1, pose1); CollisionObject co2(m2, pose2); - hpp::fcl::collide(&co1, &co2, request, result); + coal::collide(&co1, &co2, request, result); distance = result.distance_lower_bound; return result.isCollision(); } -bool testCollide(const Transform3f& tf, const CollisionGeometryPtr_t& m1, +bool testCollide(const Transform3s& tf, const CollisionGeometryPtr_t& m1, const CollisionGeometryPtr_t& m2) { - Transform3f pose1(tf), pose2; + Transform3s pose1(tf), pose2; - CollisionRequest request(hpp::fcl::NO_REQUEST, 1); + CollisionRequest request(coal::NO_REQUEST, 1); request.enable_distance_lower_bound = false; CollisionResult result; CollisionObject co1(m1, pose1); CollisionObject co2(m2, pose2); - hpp::fcl::collide(&co1, &co2, request, result); + coal::collide(&co1, &co2, request, result); return result.isCollision(); } -bool testDistance(const Transform3f& tf, const CollisionGeometryPtr_t& m1, - const CollisionGeometryPtr_t& m2, FCL_REAL& distance) { - Transform3f pose1(tf), pose2; +bool testDistance(const Transform3s& tf, const CollisionGeometryPtr_t& m1, + const CollisionGeometryPtr_t& m2, CoalScalar& distance) { + Transform3s pose1(tf), pose2; DistanceRequest request; DistanceResult result; CollisionObject co1(m1, pose1); CollisionObject co2(m2, pose2); - hpp::fcl::distance(&co1, &co2, request, result); + coal::distance(&co1, &co2, request, result); distance = result.min_distance; if (result.min_distance <= 0) { @@ -113,7 +113,7 @@ bool testDistance(const Transform3f& tf, const CollisionGeometryPtr_t& m1, } BOOST_AUTO_TEST_CASE(mesh_mesh) { - std::vector p1, p2; + std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); @@ -131,15 +131,15 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) { m2->addSubModel(p2, t2); m2->endModel(); - std::vector transforms; - FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; + std::vector transforms; + CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; std::size_t n = 100; generateRandomTransforms(extents, transforms, n); // collision for (std::size_t i = 0; i < transforms.size(); ++i) { - FCL_REAL distanceLowerBound, distance; + CoalScalar distanceLowerBound, distance; bool col1, col2, col3; col1 = testDistanceLowerBound(transforms[i], m1, m2, distanceLowerBound); col2 = testDistance(transforms[i], m1, m2, distance); @@ -158,21 +158,21 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) { } BOOST_AUTO_TEST_CASE(box_sphere) { - shared_ptr sphere(new hpp::fcl::Sphere(0.5)); - shared_ptr box(new hpp::fcl::Box(1., 1., 1.)); + shared_ptr sphere(new coal::Sphere(0.5)); + shared_ptr box(new coal::Box(1., 1., 1.)); - Transform3f M1; + Transform3s M1; M1.setIdentity(); - Transform3f M2; + Transform3s M2; M2.setIdentity(); - std::vector transforms; - FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.}; + std::vector transforms; + CoalScalar extents[] = {-2., -2., -2., 2., 2., 2.}; const std::size_t n = 1000; generateRandomTransforms(extents, transforms, n); - FCL_REAL distanceLowerBound, distance; + CoalScalar distanceLowerBound, distance; bool col1, col2; col1 = testDistanceLowerBound(M1, sphere, box, distanceLowerBound); col2 = testDistance(M1, sphere, box, distance); @@ -180,7 +180,7 @@ BOOST_AUTO_TEST_CASE(box_sphere) { BOOST_CHECK(distanceLowerBound <= distance); for (std::size_t i = 0; i < transforms.size(); ++i) { - FCL_REAL distanceLowerBound, distance; + CoalScalar distanceLowerBound, distance; bool col1, col2; col1 = testDistanceLowerBound(transforms[i], sphere, box, distanceLowerBound); @@ -195,21 +195,21 @@ BOOST_AUTO_TEST_CASE(box_sphere) { } BOOST_AUTO_TEST_CASE(sphere_sphere) { - shared_ptr sphere1(new hpp::fcl::Sphere(0.5)); - shared_ptr sphere2(new hpp::fcl::Sphere(1.)); + shared_ptr sphere1(new coal::Sphere(0.5)); + shared_ptr sphere2(new coal::Sphere(1.)); - Transform3f M1; + Transform3s M1; M1.setIdentity(); - Transform3f M2; + Transform3s M2; M2.setIdentity(); - std::vector transforms; - FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.}; + std::vector transforms; + CoalScalar extents[] = {-2., -2., -2., 2., 2., 2.}; const std::size_t n = 1000; generateRandomTransforms(extents, transforms, n); - FCL_REAL distanceLowerBound, distance; + CoalScalar distanceLowerBound, distance; bool col1, col2; col1 = testDistanceLowerBound(M1, sphere1, sphere2, distanceLowerBound); col2 = testDistance(M1, sphere1, sphere2, distance); @@ -217,7 +217,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { BOOST_CHECK(distanceLowerBound <= distance); for (std::size_t i = 0; i < transforms.size(); ++i) { - FCL_REAL distanceLowerBound, distance; + CoalScalar distanceLowerBound, distance; bool col1, col2; col1 = testDistanceLowerBound(transforms[i], sphere1, sphere2, distanceLowerBound); @@ -232,28 +232,28 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { } BOOST_AUTO_TEST_CASE(box_mesh) { - std::vector p1; + std::vector p1; std::vector t1; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); shared_ptr > m1(new BVHModel); - shared_ptr m2(new hpp::fcl::Box(500, 200, 150)); + shared_ptr m2(new coal::Box(500, 200, 150)); m1->beginModel(); m1->addSubModel(p1, t1); m1->endModel(); - std::vector transforms; - FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; + std::vector transforms; + CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; std::size_t n = 100; generateRandomTransforms(extents, transforms, n); // collision for (std::size_t i = 0; i < transforms.size(); ++i) { - FCL_REAL distanceLowerBound, distance; + CoalScalar distanceLowerBound, distance; bool col1, col2; col1 = testDistanceLowerBound(transforms[i], m1, m2, distanceLowerBound); col2 = testDistance(transforms[i], m1, m2, distance); diff --git a/test/frontlist.cpp b/test/frontlist.cpp index 724144faa..9e68a326c 100644 --- a/test/frontlist.cpp +++ b/test/frontlist.cpp @@ -35,59 +35,59 @@ /** \author Jia Pan */ -#define BOOST_TEST_MODULE FCL_FRONT_LIST +#define BOOST_TEST_MODULE COAL_FRONT_LIST #include -#include -#include +#include "coal/internal/traversal_node_bvhs.h" +#include "coal/internal/traversal_node_setup.h" #include <../src/collision_node.h> -#include +#include "coal/internal/BV_splitter.h" #include "utility.h" #include "fcl_resources/config.h" #include -using namespace hpp::fcl; +using namespace coal; namespace utf = boost::unit_test::framework; template -bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, - const std::vector& vertices1, +bool collide_front_list_Test(const Transform3s& tf1, const Transform3s& tf2, + const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool refit_bottomup, bool verbose); template -bool collide_front_list_Test_Oriented(const Transform3f& tf1, - const Transform3f& tf2, - const std::vector& vertices1, +bool collide_front_list_Test_Oriented(const Transform3s& tf1, + const Transform3s& tf2, + const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); template -bool collide_Test(const Transform3f& tf, const std::vector& vertices1, +bool collide_Test(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); // TODO: randomly still have some runtime error BOOST_AUTO_TEST_CASE(front_list) { - std::vector p1, p2; + std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); - std::vector transforms; // t0 - std::vector transforms2; // t1 - FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; - FCL_REAL delta_trans[] = {1, 1, 1}; + std::vector transforms; // t0 + std::vector transforms2; // t1 + CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; + CoalScalar delta_trans[] = {1, 1, 1}; #ifndef NDEBUG // if debug mode std::size_t n = 2; #else @@ -270,10 +270,10 @@ BOOST_AUTO_TEST_CASE(front_list) { } template -bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, - const std::vector& vertices1, +bool collide_front_list_Test(const Transform3s& tf1, const Transform3s& tf2, + const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool refit_bottomup, bool verbose) { @@ -284,7 +284,7 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, BVHFrontList front_list; - std::vector vertices1_new(vertices1.size()); + std::vector vertices1_new(vertices1.size()); for (std::size_t i = 0; i < vertices1_new.size(); ++i) { vertices1_new[i] = tf1.transform(vertices1[i]); } @@ -297,7 +297,7 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, m2.addSubModel(vertices2, triangles2); m2.endModel(); - Transform3f pose1, pose2; + Transform3s pose1, pose2; CollisionResult local_result; CollisionRequest request(NO_REQUEST, (std::numeric_limits::max)()); @@ -336,11 +336,11 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, } template -bool collide_front_list_Test_Oriented(const Transform3f& tf1, - const Transform3f& tf2, - const std::vector& vertices1, +bool collide_front_list_Test_Oriented(const Transform3s& tf1, + const Transform3s& tf2, + const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { @@ -359,7 +359,7 @@ bool collide_front_list_Test_Oriented(const Transform3f& tf1, m2.addSubModel(vertices2, triangles2); m2.endModel(); - Transform3f pose1(tf1), pose2; + Transform3s pose1(tf1), pose2; CollisionResult local_result; CollisionRequest request(NO_REQUEST, (std::numeric_limits::max)()); @@ -392,9 +392,9 @@ bool collide_front_list_Test_Oriented(const Transform3f& tf1, } template -bool collide_Test(const Transform3f& tf, const std::vector& vertices1, +bool collide_Test(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { BVHModel m1; @@ -410,7 +410,7 @@ bool collide_Test(const Transform3f& tf, const std::vector& vertices1, m2.addSubModel(vertices2, triangles2); m2.endModel(); - Transform3f pose1(tf), pose2; + Transform3s pose1(tf), pose2; CollisionResult local_result; CollisionRequest request(NO_REQUEST, (std::numeric_limits::max)()); diff --git a/test/geometric_shapes.cpp b/test/geometric_shapes.cpp index ec7425248..ec8e6b468 100644 --- a/test/geometric_shapes.cpp +++ b/test/geometric_shapes.cpp @@ -36,23 +36,23 @@ /** \author Jia Pan */ -#define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES +#define BOOST_TEST_MODULE COAL_GEOMETRIC_SHAPES #include -#include -#include -#include +#include "coal/narrowphase/narrowphase.h" +#include "coal/collision.h" +#include "coal/distance.h" #include "utility.h" #include -#include -#include -#include +#include "coal/internal/tools.h" +#include "coal/shape/geometric_shape_to_BVH_model.h" +#include "coal/internal/shape_shape_func.h" -using namespace hpp::fcl; +using namespace coal; -FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10}; +CoalScalar extents[6] = {0, 0, 0, 10, 10, 10}; -FCL_REAL tol_gjk = 0.01; +CoalScalar tol_gjk = 0.01; GJKSolver solver1; GJKSolver solver2; @@ -66,8 +66,7 @@ int line; << "].") #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p)) -namespace hpp { -namespace fcl { +namespace coal { std::ostream& operator<<(std::ostream& os, const ShapeBase&) { return os << "a_shape"; } @@ -75,16 +74,15 @@ std::ostream& operator<<(std::ostream& os, const ShapeBase&) { std::ostream& operator<<(std::ostream& os, const Box& b) { return os << "Box(" << 2 * b.halfSide.transpose() << ')'; } -} // namespace fcl -} // namespace hpp +} // namespace coal template void printComparisonError(const std::string& comparison_type, const S1& s1, - const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, - const Vec3f& contact_or_normal, - const Vec3f& expected_contact_or_normal, - bool check_opposite_normal, FCL_REAL tol) { + const Transform3s& tf1, const S2& s2, + const Transform3s& tf2, + const Vec3s& contact_or_normal, + const Vec3s& expected_contact_or_normal, + bool check_opposite_normal, CoalScalar tol) { std::cout << "Disagreement between " << comparison_type << " and expected_" << comparison_type << " for " << getNodeTypeName(s1.getNodeType()) << " and " << getNodeTypeName(s2.getNodeType()) << ".\n" @@ -111,9 +109,9 @@ void printComparisonError(const std::string& comparison_type, const S1& s1, template void printComparisonError(const std::string& comparison_type, const S1& s1, - const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, FCL_REAL depth, - FCL_REAL expected_depth, FCL_REAL tol) { + const Transform3s& tf1, const S2& s2, + const Transform3s& tf2, CoalScalar depth, + CoalScalar expected_depth, CoalScalar tol) { std::cout << "Disagreement between " << comparison_type << " and expected_" << comparison_type << " for " << getNodeTypeName(s1.getNodeType()) << " and " << getNodeTypeName(s2.getNodeType()) << ".\n" @@ -128,12 +126,12 @@ void printComparisonError(const std::string& comparison_type, const S1& s1, } template -void compareContact(const S1& s1, const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, const Vec3f& contact, - Vec3f* expected_point, FCL_REAL depth, - FCL_REAL* expected_depth, const Vec3f& normal, - Vec3f* expected_normal, bool check_opposite_normal, - FCL_REAL tol) { +void compareContact(const S1& s1, const Transform3s& tf1, const S2& s2, + const Transform3s& tf2, const Vec3s& contact, + Vec3s* expected_point, CoalScalar depth, + CoalScalar* expected_depth, const Vec3s& normal, + Vec3s* expected_normal, bool check_opposite_normal, + CoalScalar tol) { if (expected_point) { bool contact_equal = isEqual(contact, *expected_point, tol); FCL_CHECK(contact_equal); @@ -164,17 +162,18 @@ void compareContact(const S1& s1, const Transform3f& tf1, const S2& s2, } template -void testShapeCollide(const S1& s1, const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, bool expect_collision, - Vec3f* expected_point = NULL, - FCL_REAL* expected_depth = NULL, - Vec3f* expected_normal = NULL, - bool check_opposite_normal = false, FCL_REAL tol = 1e-9) { +void testShapeCollide(const S1& s1, const Transform3s& tf1, const S2& s2, + const Transform3s& tf2, bool expect_collision, + Vec3s* expected_point = NULL, + CoalScalar* expected_depth = NULL, + Vec3s* expected_normal = NULL, + bool check_opposite_normal = false, + CoalScalar tol = 1e-9) { CollisionRequest request; CollisionResult result; - Vec3f contact; - Vec3f normal; // normal direction should be from object 1 to object 2 + Vec3s contact; + Vec3s normal; // normal direction should be from object 1 to object 2 bool collision; bool check_failed = false; @@ -211,85 +210,85 @@ void testShapeCollide(const S1& s1, const Transform3f& tf1, const S2& s2, BOOST_AUTO_TEST_CASE(box_to_bvh) { Box shape(1, 1, 1); BVHModel bvh; - generateBVHModel(bvh, shape, Transform3f()); + generateBVHModel(bvh, shape, Transform3s()); } BOOST_AUTO_TEST_CASE(sphere_to_bvh) { Sphere shape(1); BVHModel bvh; - generateBVHModel(bvh, shape, Transform3f(), 10, 10); - generateBVHModel(bvh, shape, Transform3f(), 50); + generateBVHModel(bvh, shape, Transform3s(), 10, 10); + generateBVHModel(bvh, shape, Transform3s(), 50); } BOOST_AUTO_TEST_CASE(cylinder_to_bvh) { Cylinder shape(1, 1); BVHModel bvh; - generateBVHModel(bvh, shape, Transform3f(), 10, 10); - generateBVHModel(bvh, shape, Transform3f(), 50); + generateBVHModel(bvh, shape, Transform3s(), 10, 10); + generateBVHModel(bvh, shape, Transform3s(), 50); } BOOST_AUTO_TEST_CASE(cone_to_bvh) { Cone shape(1, 1); BVHModel bvh; - generateBVHModel(bvh, shape, Transform3f(), 10, 10); - generateBVHModel(bvh, shape, Transform3f(), 50); + generateBVHModel(bvh, shape, Transform3s(), 10, 10); + generateBVHModel(bvh, shape, Transform3s(), 50); } BOOST_AUTO_TEST_CASE(collide_spheresphere) { Sphere s1(20); Sphere s2(10); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + // Vec3s point; + // CoalScalar depth; + Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(40, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(40, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(40, 0, 0)); + tf2 = transform * Transform3s(Vec3s(40, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(30, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(30, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(30.01, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(30.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(30.01, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); + tf2 = transform * Transform3s(Vec3s(30.01, 0, 0)); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(29.9, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(29.9, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(29.9, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); + tf2 = transform * Transform3s(Vec3s(29.9, 0, 0)); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); normal << 1, 0, 0; // If the centers of two sphere are at the same position, // the normal is (1, 0, 0) SET_LINE; @@ -302,46 +301,46 @@ BOOST_AUTO_TEST_CASE(collide_spheresphere) { SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-29.9, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-29.9, 0, 0)); normal << -1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0)); - normal = transform.getRotation() * Vec3f(-1, 0, 0); + tf2 = transform * Transform3s(Vec3s(-29.9, 0, 0)); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-30.0, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-30.0, 0, 0)); normal << -1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-30.01, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-30.01, 0, 0)); normal << -1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-30.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); } -bool compareContactPoints(const Vec3f& c1, const Vec3f& c2) { +bool compareContactPoints(const Vec3s& c1, const Vec3s& c2) { return c1[2] < c2[2]; } // Ascending order -void testBoxBoxContactPoints(const Matrix3f& R) { +void testBoxBoxContactPoints(const Matrix3s& R) { Box s1(100, 100, 100); Box s2(10, 20, 30); // Vertices of s2 - std::vector vertices(8); + std::vector vertices(8); vertices[0] << 1, 1, 1; vertices[1] << 1, 1, -1; vertices[2] << 1, -1, 1; @@ -355,17 +354,17 @@ void testBoxBoxContactPoints(const Matrix3f& R) { vertices[i].array() *= s2.halfSide.array(); } - Transform3f tf1 = Transform3f(Vec3f(0, 0, -50)); - Transform3f tf2 = Transform3f(R); + Transform3s tf1 = Transform3s(Vec3s(0, 0, -50)); + Transform3s tf2 = Transform3s(R); - Vec3f normal; - Vec3f p1, p2; + Vec3s normal; + Vec3s p1, p2; // Make sure the two boxes are colliding solver1.gjk_tolerance = 1e-5; solver1.epa_tolerance = 1e-5; const bool compute_penetration = true; - FCL_REAL distance = solver1.shapeDistance( + CoalScalar distance = solver1.shapeDistance( s1, tf1, s2, tf2, compute_penetration, p1, p2, normal); FCL_CHECK(distance <= 0); @@ -376,8 +375,8 @@ void testBoxBoxContactPoints(const Matrix3f& R) { std::sort(vertices.begin(), vertices.end(), compareContactPoints); // The lowest vertex along z-axis should be the contact point - FCL_CHECK(normal.isApprox(Vec3f(0, 0, 1), 1e-6)); - Vec3f point = 0.5 * (p1 + p2); + FCL_CHECK(normal.isApprox(Vec3s(0, 0, 1), 1e-6)); + Vec3s point = 0.5 * (p1 + p2); FCL_CHECK(vertices[0].head<2>().isApprox(point.head<2>(), 1e-6)); FCL_CHECK(vertices[0][2] <= point[2] && point[2] < 0); } @@ -386,21 +385,21 @@ BOOST_AUTO_TEST_CASE(collide_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + // Vec3s point; + // CoalScalar depth; + Vec3s normal; Quatf q; - q = AngleAxis((FCL_REAL)3.140 / 6, UnitZ); + q = AngleAxis((CoalScalar)3.140 / 6, UnitZ); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. The current result is (1, 0, 0). normal << 1, 0, 0; @@ -411,41 +410,41 @@ BOOST_AUTO_TEST_CASE(collide_boxbox) { tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at // same position. The current result is (1, 0, 0). - normal = transform.getRotation() * Vec3f(1, 0, 0); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(15, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(15, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, 1e-8); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(15.01, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(15.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(15.01, 0, 0)); + tf2 = transform * Transform3s(Vec3s(15.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(q); + tf1 = Transform3s(); + tf2 = Transform3s(q); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); tf1 = transform; - tf2 = transform * Transform3f(q); - normal = transform.getRotation() * Vec3f(1, 0, 0); + tf2 = transform * Transform3s(q); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); int numTests = 100; for (int i = 0; i < numTests; ++i) { - Transform3f tf; + Transform3s tf; generateRandomTransform(extents, tf); SET_LINE; testBoxBoxContactPoints(tf.getRotation()); @@ -456,18 +455,18 @@ BOOST_AUTO_TEST_CASE(collide_spherebox) { Sphere s1(20); Box s2(5, 5, 5); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + // Vec3s point; + // CoalScalar depth; + Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. The current result is (-1, 0, 0). normal << -1, 0, 0; @@ -481,68 +480,68 @@ BOOST_AUTO_TEST_CASE(collide_spherebox) { SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(22.50001, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(22.50001, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(22.501, 0, 0)); + tf2 = transform * Transform3s(Vec3s(22.501, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(22.4, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(22.4, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(22.4, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); + tf2 = transform * Transform3s(Vec3s(22.4, 0, 0)); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); } BOOST_AUTO_TEST_CASE(distance_spherebox) { - hpp::fcl::Matrix3f rotSphere; + coal::Matrix3s rotSphere; rotSphere << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; - hpp::fcl::Vec3f trSphere(0.0, 0.0, 0.0); + coal::Vec3s trSphere(0.0, 0.0, 0.0); - hpp::fcl::Matrix3f rotBox; + coal::Matrix3s rotBox; rotBox << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; - hpp::fcl::Vec3f trBox(0.0, 5.0, 3.0); + coal::Vec3s trBox(0.0, 5.0, 3.0); - hpp::fcl::Sphere sphere(1); - hpp::fcl::Box box(10, 2, 10); + coal::Sphere sphere(1); + coal::Box box(10, 2, 10); - hpp::fcl::DistanceResult result; - distance(&sphere, Transform3f(rotSphere, trSphere), &box, - Transform3f(rotBox, trBox), DistanceRequest(true), result); + coal::DistanceResult result; + distance(&sphere, Transform3s(rotSphere, trSphere), &box, + Transform3s(rotBox, trBox), DistanceRequest(true), result); - FCL_REAL eps = Eigen::NumTraits::epsilon(); + CoalScalar eps = Eigen::NumTraits::epsilon(); BOOST_CHECK_CLOSE(result.min_distance, 3., eps); - EIGEN_VECTOR_IS_APPROX(result.nearest_points[0], Vec3f(0, 1, 0), eps); - EIGEN_VECTOR_IS_APPROX(result.nearest_points[1], Vec3f(0, 4, 0), eps); - EIGEN_VECTOR_IS_APPROX(result.normal, Vec3f(0, 1, 0), eps); + EIGEN_VECTOR_IS_APPROX(result.nearest_points[0], Vec3s(0, 1, 0), eps); + EIGEN_VECTOR_IS_APPROX(result.nearest_points[1], Vec3s(0, 4, 0), eps); + EIGEN_VECTOR_IS_APPROX(result.normal, Vec3s(0, 1, 0), eps); } BOOST_AUTO_TEST_CASE(collide_spherecapsule) { Sphere s1(20); Capsule s2(5, 10); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + // Vec3s point; + // CoalScalar depth; + Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; @@ -555,39 +554,39 @@ BOOST_AUTO_TEST_CASE(collide_spherecapsule) { SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(24.9, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(24.9, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(24.9, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); + tf2 = transform * Transform3s(Vec3s(24.9, 0, 0)); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(25, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(25, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(24.999999, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); + tf2 = transform * Transform3s(Vec3s(24.999999, 0, 0)); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(25.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(25.1, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(25.1, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); + tf2 = transform * Transform3s(Vec3s(25.1, 0, 0)); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); } @@ -596,18 +595,18 @@ BOOST_AUTO_TEST_CASE(collide_cylindercylinder) { Cylinder s1(5, 15); Cylinder s2(5, 15); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + // Vec3s point; + // CoalScalar depth; + Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; @@ -620,37 +619,37 @@ BOOST_AUTO_TEST_CASE(collide_cylindercylinder) { SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(9.9, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(9.9, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 9.9, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 9.9, 0)); normal << 0, 1, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(9.9, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(9.9, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); + tf2 = transform * Transform3s(Vec3s(9.9, 0, 0)); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(10.01, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(10.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(10.01, 0, 0)); + tf2 = transform * Transform3s(Vec3s(10.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); } @@ -659,18 +658,18 @@ BOOST_AUTO_TEST_CASE(collide_conecone) { Cone s1(5, 10); Cone s2(5, 10); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + // Vec3s point; + // CoalScalar depth; + Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; @@ -683,10 +682,10 @@ BOOST_AUTO_TEST_CASE(collide_conecone) { SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); - tf1 = Transform3f(); + tf1 = Transform3s(); // z=0 is a singular points. Two normals could be returned. - tf2 = Transform3f(Vec3f(9.9, 0, 0.00001)); - normal = Vec3f(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius) + tf2 = Transform3s(Vec3s(9.9, 0, 0.00001)); + normal = Vec3s(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius) .normalized(); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); @@ -698,37 +697,37 @@ BOOST_AUTO_TEST_CASE(collide_conecone) { testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(9.9, 0, 0.00001)); - normal = Vec3f(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius) + tf2 = transform * Transform3s(Vec3s(9.9, 0, 0.00001)); + normal = Vec3s(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius) .normalized(); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, true, tol_gjk); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(10.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(10.1, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(10.001, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(10.001, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(10.001, 0, 0)); + tf2 = transform * Transform3s(Vec3s(10.001, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 9.9)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 9.9)); normal << 0, 0, 1; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); - normal = transform.getRotation() * Vec3f(0, 0, 1); + tf2 = transform * Transform3s(Vec3s(0, 0, 9.9)); + normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); } @@ -737,18 +736,18 @@ BOOST_AUTO_TEST_CASE(collide_conecylinder) { Cylinder s1(5, 10); Cone s2(5, 10); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + // Vec3s point; + // CoalScalar depth; + Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; @@ -761,82 +760,82 @@ BOOST_AUTO_TEST_CASE(collide_conecylinder) { SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(9.9, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(9.9, 0, 0)); normal = - Vec3f(2 * (s1.halfLength + s2.halfLength), 0, -(s1.radius + s2.radius)) + Vec3s(2 * (s1.halfLength + s2.halfLength), 0, -(s1.radius + s2.radius)) .normalized(); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); + tf2 = transform * Transform3s(Vec3s(9.9, 0, 0)); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(9.9, 0, 0.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(9.9, 0, 0.1)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(9.9, 0, 0.1)); + tf2 = transform * Transform3s(Vec3s(9.9, 0, 0.1)); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(10.01, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(10.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(10.01, 0, 0)); + tf2 = transform * Transform3s(Vec3s(10.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(10, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(10, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(10, 0, 0)); + tf2 = transform * Transform3s(Vec3s(10, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 9.9)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 9.9)); normal << 0, 0, 1; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); - normal = transform.getRotation() * Vec3f(0, 0, 1); + tf2 = transform * Transform3s(Vec3s(0, 0, 9.9)); + normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 10.01)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 10.01)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 10.01)); + tf2 = transform * Transform3s(Vec3s(0, 0, 10.01)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 10)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 10)); normal << 0, 0, 1; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); } @@ -844,35 +843,35 @@ BOOST_AUTO_TEST_CASE(collide_conecylinder) { BOOST_AUTO_TEST_CASE(collide_spheretriangle) { Sphere s(10); - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - Vec3f normal; + Vec3s normal; // // Testing collision x, y, z // { - Vec3f t[3]; + Vec3s t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 20, 0; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); // identity + Transform3s tf_tri = Transform3s(); // identity - tf_tri.setTranslation(Vec3f(0, 0, 0.001)); + tf_tri.setTranslation(Vec3s(0, 0, 0.001)); normal << 0, 0, 1; SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, 0, -0.001)); + tf_tri.setTranslation(Vec3s(0, 0, -0.001)); normal << 0, 0, -1; SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -880,28 +879,28 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 30, 0, 0; t[1] << 9.9, -20, 0; t[2] << 9.9, 20, 0; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); + Transform3s tf_tri = Transform3s(); - tf_tri.setTranslation(Vec3f(0, 0, 0.001)); + tf_tri.setTranslation(Vec3s(0, 0, 0.001)); normal << 9.9, 0, 0.001; normal.normalize(); SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, 0, -0.001)); + tf_tri.setTranslation(Vec3s(0, 0, -0.001)); normal << 9.9, 0, -0.001; normal.normalize(); SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -909,26 +908,26 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 30, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); + Transform3s tf_tri = Transform3s(); - tf_tri.setTranslation(Vec3f(0, 0.001, 0)); + tf_tri.setTranslation(Vec3s(0, 0.001, 0)); normal << 0, 1, 0; SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, -0.001, 0)); + tf_tri.setTranslation(Vec3s(0, -0.001, 0)); normal << 0, -1, 0; SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -936,25 +935,25 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 0, 30, 0; t[1] << 0, -10, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); + Transform3s tf_tri = Transform3s(); - tf_tri.setTranslation(Vec3f(0.001, 0, 0)); + tf_tri.setTranslation(Vec3s(0.001, 0, 0)); normal << 1, 0, 0; SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(-0.001, 0, 0)); + tf_tri.setTranslation(Vec3s(-0.001, 0, 0)); normal << -1, 0, 0; - testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); SET_LINE; normal = transform.getRotation() * normal; SET_LINE; @@ -966,307 +965,307 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { // Testing no collision x, y, z // { - Vec3f t[3]; + Vec3s t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 20, 0; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); + Transform3s tf_tri = Transform3s(); - tf_tri.setTranslation(Vec3f(0, 0, 10.1)); + tf_tri.setTranslation(Vec3s(0, 0, 10.1)); SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, false); + testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(0, 0, -10.1)); + tf_tri.setTranslation(Vec3s(0, 0, -10.1)); SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, false); + testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); - tf_tri.setTranslation(Vec3f(0, 10.1, 0)); + Transform3s tf_tri = Transform3s(); + tf_tri.setTranslation(Vec3s(0, 10.1, 0)); SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, false); + testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(0, -10.1, 0)); + tf_tri.setTranslation(Vec3s(0, -10.1, 0)); SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, false); + testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 0, 20, 0; t[1] << 0, -20, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); + Transform3s tf_tri = Transform3s(); - tf_tri.setTranslation(Vec3f(10.1, 0, 0)); + tf_tri.setTranslation(Vec3s(10.1, 0, 0)); SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, false); + testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(-10.1, 0, 0)); + tf_tri.setTranslation(Vec3s(-10.1, 0, 0)); SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, false); + testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); } } BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) { - Halfspace hs(Vec3f(0, 0, 1), 0); + Halfspace hs(Vec3s(0, 0, 1), 0); - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - Vec3f normal; + Vec3s normal; normal = hs.n; // with halfspaces, it's simple: normal is always // the normal of the halfspace. { - Vec3f t[3]; + Vec3s t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 20, 0; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); // identity + Transform3s tf_tri = Transform3s(); // identity - tf_tri.setTranslation(Vec3f(0, 0, -0.001)); + tf_tri.setTranslation(Vec3s(0, 0, -0.001)); normal = hs.n; SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(hs, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, 0, 0.001)); + tf_tri.setTranslation(Vec3s(0, 0, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(1, 1, 0.001)); + tf_tri.setTranslation(Vec3s(1, 1, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(-1, -1, 0.001)); + tf_tri.setTranslation(Vec3s(-1, -1, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 30, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); + Transform3s tf_tri = Transform3s(); - tf_tri.setTranslation(Vec3f(0, 0, -0.001)); + tf_tri.setTranslation(Vec3s(0, 0, -0.001)); normal = hs.n; SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(hs, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, 0, 0.001)); + tf_tri.setTranslation(Vec3s(0, 0, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(1, 1, 0.001)); + tf_tri.setTranslation(Vec3s(1, 1, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(-1, -1, 0.001)); + tf_tri.setTranslation(Vec3s(-1, -1, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 0, 30, 0; t[1] << 0, -10, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); + Transform3s tf_tri = Transform3s(); - tf_tri.setTranslation(Vec3f(0, 0, -0.001)); + tf_tri.setTranslation(Vec3s(0, 0, -0.001)); normal = hs.n; SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(hs, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, 0, 0.001)); + tf_tri.setTranslation(Vec3s(0, 0, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(1, 1, 0.001)); + tf_tri.setTranslation(Vec3s(1, 1, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(-1, -1, 0.001)); + tf_tri.setTranslation(Vec3s(-1, -1, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); } } BOOST_AUTO_TEST_CASE(collide_planetriangle) { - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - Vec3f normal; + Vec3s normal; { - Vec3f t[3]; + Vec3s t[3]; t[0] << 20, 0, 0.05; t[1] << -20, 0, 0.05; t[2] << 0, 20, -0.1; - Plane pl(Vec3f(0, 0, 1), 0); + Plane pl(Vec3s(0, 0, 1), 0); TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); // identity + Transform3s tf_tri = Transform3s(); // identity normal = -pl.n; SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, 0, 0.05)); + tf_tri.setTranslation(Vec3s(0, 0, 0.05)); normal = pl.n; SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, 0, -0.06)); + tf_tri.setTranslation(Vec3s(0, 0, -0.06)); SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, false); + testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(0, 0, 0.11)); + tf_tri.setTranslation(Vec3s(0, 0, 0.11)); SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, false); + testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 30, 0.05, 0; t[1] << -20, 0.05, 0; t[2] << 0, -0.1, 20; - Plane pl(Vec3f(0, 1, 0), 0); + Plane pl(Vec3s(0, 1, 0), 0); TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); // identity + Transform3s tf_tri = Transform3s(); // identity normal = -pl.n; SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, 0.05, 0)); + tf_tri.setTranslation(Vec3s(0, 0.05, 0)); normal = pl.n; SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, -0.06, 0)); + tf_tri.setTranslation(Vec3s(0, -0.06, 0)); SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, false); + testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(0, 0.11, 0)); + tf_tri.setTranslation(Vec3s(0, 0.11, 0)); SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, false); + testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 0.05, 30, 0; t[1] << 0.05, -10, 0; t[2] << -0.1, 0, 20; - Plane pl(Vec3f(1, 0, 0), 0); + Plane pl(Vec3s(1, 0, 0), 0); TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); // identity + Transform3s tf_tri = Transform3s(); // identity normal = -pl.n; SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0.05, 0, 0)); + tf_tri.setTranslation(Vec3s(0.05, 0, 0)); normal = pl.n; SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(-0.06, 0, 0)); + tf_tri.setTranslation(Vec3s(-0.06, 0, 0)); SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, false); + testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(0.11, 0, 0)); + tf_tri.setTranslation(Vec3s(0.11, 0, 0)); SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, false); + testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); } @@ -1274,20 +1273,20 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { Sphere s(10); - Halfspace hs(Vec3f(1, 0, 0), 0); + Halfspace hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + Vec3s contact; + CoalScalar depth; + Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << -5, 0, 0; depth = -10; normal << -1, 0, 0; @@ -1296,14 +1295,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(-5, 0, 0)); + contact = transform.transform(Vec3s(-5, 0, 0)); depth = -10; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(5, 0, 0)); contact << -2.5, 0, 0; depth = -15; normal << -1, 0, 0; @@ -1311,15 +1310,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(5, 0, 0)); - contact = transform.transform(Vec3f(-2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(5, 0, 0)); + contact = transform.transform(Vec3s(-2.5, 0, 0)); depth = -15; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-5, 0, 0)); contact << -7.5, 0, 0; depth = -5; normal << -1, 0, 0; @@ -1327,25 +1326,25 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-5, 0, 0)); - contact = transform.transform(Vec3f(-7.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-5, 0, 0)); + contact = transform.transform(Vec3s(-7.5, 0, 0)); depth = -5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-10.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(10.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(10.1, 0, 0)); contact << 0.05, 0, 0; depth = -20.1; normal << -1, 0, 0; @@ -1353,32 +1352,32 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(10.1, 0, 0)); - contact = transform.transform(Vec3f(0.05, 0, 0)); + tf2 = transform * Transform3s(Vec3s(10.1, 0, 0)); + contact = transform.transform(Vec3s(0.05, 0, 0)); depth = -20.1; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); } BOOST_AUTO_TEST_CASE(collide_planesphere) { Sphere s(10); - Plane hs(Vec3f(1, 0, 0), 0); + Plane hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; - Vec3f p1, p2; + Vec3s contact; + CoalScalar depth; + Vec3s normal; + Vec3s p1, p2; - FCL_REAL eps = 1e-6; - tf1 = Transform3f(Vec3f(eps, 0, 0)); - tf2 = Transform3f(); + CoalScalar eps = 1e-6; + tf1 = Transform3s(Vec3s(eps, 0, 0)); + tf2 = Transform3s(); depth = -10 + eps; p1 << -10 + eps, 0, 0; p2 << 0, 0, 0; @@ -1391,13 +1390,13 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); normal = - transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3f(eps, 0, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(eps, 0, 0)); + tf2 = Transform3s(); depth = -10 - eps; p1 << 10 + eps, 0, 0; p2 << 0, 0, 0; @@ -1409,12 +1408,12 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) { tf1 = transform * tf1; tf2 = transform; contact = transform.transform((p1 + p2) / 2); - normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(5, 0, 0)); p1 << 10, 0, 0; p2 << 5, 0, 0; contact << (p1 + p2) / 2; @@ -1424,15 +1423,15 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -5; - normal = transform.getRotation() * Vec3f(1, 0, 0); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-5, 0, 0)); p1 << -10, 0, 0; p2 << -5, 0, 0; contact << (p1 + p2) / 2; @@ -1442,50 +1441,50 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-10.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(10.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(10.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_halfspacebox) { Box s(5, 10, 20); - Halfspace hs(Vec3f(1, 0, 0), 0); + Halfspace hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + Vec3s contact; + CoalScalar depth; + Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << -1.25, 0, 0; depth = -2.5; normal << -1, 0, 0; @@ -1494,14 +1493,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(-1.25, 0, 0)); + contact = transform.transform(Vec3s(-1.25, 0, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(1.25, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(1.25, 0, 0)); contact << -0.625, 0, 0; depth = -3.75; normal << -1, 0, 0; @@ -1509,15 +1508,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(1.25, 0, 0)); - contact = transform.transform(Vec3f(-0.625, 0, 0)); + tf2 = transform * Transform3s(Vec3s(1.25, 0, 0)); + contact = transform.transform(Vec3s(-0.625, 0, 0)); depth = -3.75; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-1.25, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-1.25, 0, 0)); contact << -1.875, 0, 0; depth = -1.25; normal << -1, 0, 0; @@ -1525,15 +1524,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0)); - contact = transform.transform(Vec3f(-1.875, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-1.25, 0, 0)); + contact = transform.transform(Vec3s(-1.875, 0, 0)); depth = -1.25; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(2.51, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(2.51, 0, 0)); contact << 0.005, 0, 0; depth = -5.01; normal << -1, 0, 0; @@ -1541,47 +1540,47 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(2.51, 0, 0)); - contact = transform.transform(Vec3f(0.005, 0, 0)); + tf2 = transform * Transform3s(Vec3s(2.51, 0, 0)); + contact = transform.transform(Vec3s(0.005, 0, 0)); depth = -5.01; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-2.51, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(transform.getRotation()); - tf2 = Transform3f(); + tf1 = Transform3s(transform.getRotation()); + tf2 = Transform3s(); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true); } BOOST_AUTO_TEST_CASE(collide_planebox) { Box s(5, 10, 20); - Plane hs(Vec3f(1, 0, 0), 0); + Plane hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + Vec3s contact; + CoalScalar depth; + Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); - Vec3f p1(2.5, 0, 0); - Vec3f p2(0, 0, 0); + tf1 = Transform3s(); + tf2 = Transform3s(); + Vec3s p1(2.5, 0, 0); + Vec3s p2(0, 0, 0); contact << (p1 + p2) / 2; depth = -2.5; normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0) @@ -1592,12 +1591,12 @@ BOOST_AUTO_TEST_CASE(collide_planebox) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(1.25, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(1.25, 0, 0)); p2 << 1.25, 0, 0; contact << (p1 + p2) / 2; depth = -1.25; @@ -1606,15 +1605,15 @@ BOOST_AUTO_TEST_CASE(collide_planebox) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(1.25, 0, 0)); + tf2 = transform * Transform3s(Vec3s(1.25, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -1.25; - normal = transform.getRotation() * Vec3f(1, 0, 0); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-1.25, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-1.25, 0, 0)); p1 << -2.5, 0, 0; p2 << -1.25, 0, 0; contact << (p1 + p2) / 2; @@ -1624,55 +1623,55 @@ BOOST_AUTO_TEST_CASE(collide_planebox) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-1.25, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -1.25; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(2.51, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(2.51, 0, 0)); + tf2 = transform * Transform3s(Vec3s(2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-2.51, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(transform.getRotation()); - tf2 = Transform3f(); + tf1 = Transform3s(transform.getRotation()); + tf2 = Transform3s(); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true); } BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { Capsule s(5, 10); - Halfspace hs(Vec3f(1, 0, 0), 0); + Halfspace hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + Vec3s contact; + CoalScalar depth; + Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << -2.5, 0, 0; depth = -5; normal << -1, 0, 0; @@ -1681,14 +1680,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(-2.5, 0, 0)); + contact = transform.transform(Vec3s(-2.5, 0, 0)); depth = -5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(2.5, 0, 0)); contact << -1.25, 0, 0; depth = -7.5; normal << -1, 0, 0; @@ -1696,15 +1695,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); - contact = transform.transform(Vec3f(-1.25, 0, 0)); + tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); + contact = transform.transform(Vec3s(-1.25, 0, 0)); depth = -7.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-2.5, 0, 0)); contact << -3.75, 0, 0; depth = -2.5; normal << -1, 0, 0; @@ -1712,15 +1711,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); - contact = transform.transform(Vec3f(-3.75, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); + contact = transform.transform(Vec3s(-3.75, 0, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(5.1, 0, 0)); contact << 0.05, 0, 0; depth = -10.1; normal << -1, 0, 0; @@ -1728,27 +1727,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); - contact = transform.transform(Vec3f(0.05, 0, 0)); + tf2 = transform * Transform3s(Vec3s(5.1, 0, 0)); + contact = transform.transform(Vec3s(0.05, 0, 0)); depth = -10.1; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Halfspace(Vec3f(0, 1, 0), 0); + hs = Halfspace(Vec3s(0, 1, 0), 0); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, -2.5, 0; depth = -5; normal << 0, -1, 0; @@ -1757,14 +1756,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, -2.5, 0)); + contact = transform.transform(Vec3s(0, -2.5, 0)); depth = -5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 2.5, 0)); contact << 0, -1.25, 0; depth = -7.5; normal << 0, -1, 0; @@ -1772,15 +1771,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); - contact = transform.transform(Vec3f(0, -1.25, 0)); + tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); + contact = transform.transform(Vec3s(0, -1.25, 0)); depth = -7.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -2.5, 0)); contact << 0, -3.75, 0; depth = -2.5; normal << 0, -1, 0; @@ -1788,15 +1787,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); - contact = transform.transform(Vec3f(0, -3.75, 0)); + tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); + contact = transform.transform(Vec3s(0, -3.75, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 5.1, 0)); contact << 0, 0.05, 0; depth = -10.1; normal << 0, -1, 0; @@ -1804,27 +1803,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); - contact = transform.transform(Vec3f(0, 0.05, 0)); + tf2 = transform * Transform3s(Vec3s(0, 5.1, 0)); + contact = transform.transform(Vec3s(0, 0.05, 0)); depth = -10.1; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Halfspace(Vec3f(0, 0, 1), 0); + hs = Halfspace(Vec3s(0, 0, 1), 0); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, 0, -5; depth = -10; normal << 0, 0, -1; @@ -1833,14 +1832,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, -5)); + contact = transform.transform(Vec3s(0, 0, -5)); depth = -10; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 2.5)); contact << 0, 0, -3.75; depth = -12.5; normal << 0, 0, -1; @@ -1848,15 +1847,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); - contact = transform.transform(Vec3f(0, 0, -3.75)); + tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); + contact = transform.transform(Vec3s(0, 0, -3.75)); depth = -12.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -2.5)); contact << 0, 0, -6.25; depth = -7.5; normal << 0, 0, -1; @@ -1864,15 +1863,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); - contact = transform.transform(Vec3f(0, 0, -6.25)); + tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); + contact = transform.transform(Vec3s(0, 0, -6.25)); depth = -7.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 10.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 10.1)); contact << 0, 0, 0.05; depth = -20.1; normal << 0, 0, -1; @@ -1880,40 +1879,40 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); - contact = transform.transform(Vec3f(0, 0, 0.05)); + tf2 = transform * Transform3s(Vec3s(0, 0, 10.1)); + contact = transform.transform(Vec3s(0, 0, 0.05)); depth = -20.1; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -10.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_planecapsule) { Capsule s(5, 10); - Plane hs(Vec3f(1, 0, 0), 0); + Plane hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + Vec3s contact; + CoalScalar depth; + Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, 0, 0; depth = -5; normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0) @@ -1922,14 +1921,14 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); + contact = transform.transform(Vec3s(0, 0, 0)); depth = -5; - normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(2.5, 0, 0)); contact << 2.5, 0, 0; depth = -2.5; normal << 1, 0, 0; @@ -1937,15 +1936,15 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); - contact = transform.transform(Vec3f(2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); + contact = transform.transform(Vec3s(2.5, 0, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(1, 0, 0); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-2.5, 0, 0)); contact << -2.5, 0, 0; depth = -2.5; normal << -1, 0, 0; @@ -1953,37 +1952,37 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); - contact = transform.transform(Vec3f(-2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); + contact = transform.transform(Vec3s(-2.5, 0, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Plane(Vec3f(0, 1, 0), 0); + hs = Plane(Vec3s(0, 1, 0), 0); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, 0, 0; depth = -5; normal << 0, 1, 0; // (0, 1, 0) or (0, -1, 0) @@ -1992,14 +1991,14 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); + contact = transform.transform(Vec3s(0, 0, 0)); depth = -5; - normal = transform.getRotation() * Vec3f(0, 1, 0); // (0, 1, 0) or (0, -1, 0) + normal = transform.getRotation() * Vec3s(0, 1, 0); // (0, 1, 0) or (0, -1, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 2.5, 0)); contact << 0, 2.5, 0; depth = -2.5; normal << 0, 1, 0; @@ -2007,15 +2006,15 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); - contact = transform.transform(Vec3f(0, 2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); + contact = transform.transform(Vec3s(0, 2.5, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 1, 0); + normal = transform.getRotation() * Vec3s(0, 1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -2.5, 0)); contact << 0, -2.5, 0; depth = -2.5; normal << 0, -1, 0; @@ -2023,37 +2022,37 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); - contact = transform.transform(Vec3f(0, -2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); + contact = transform.transform(Vec3s(0, -2.5, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Plane(Vec3f(0, 0, 1), 0); + hs = Plane(Vec3s(0, 0, 1), 0); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, 0, 0; depth = -10; normal << 0, 0, 1; // (0, 0, 1) or (0, 0, -1) @@ -2062,14 +2061,14 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); + contact = transform.transform(Vec3s(0, 0, 0)); depth = -10; - normal = transform.getRotation() * Vec3f(0, 0, 1); // (0, 0, 1) or (0, 0, -1) + normal = transform.getRotation() * Vec3s(0, 0, 1); // (0, 0, 1) or (0, 0, -1) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 2.5)); contact << 0, 0, 2.5; depth = -7.5; normal << 0, 0, 1; @@ -2077,15 +2076,15 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); - contact = transform.transform(Vec3f(0, 0, 2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); + contact = transform.transform(Vec3s(0, 0, 2.5)); depth = -7.5; - normal = transform.getRotation() * Vec3f(0, 0, 1); + normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -2.5)); contact << 0, 0, -2.5; depth = -7.5; normal << 0, 0, -1; @@ -2093,50 +2092,50 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); - contact = transform.transform(Vec3f(0, 0, -2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); + contact = transform.transform(Vec3s(0, 0, -2.5)); depth = -7.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 10.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -10.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { Cylinder s(5, 10); - Halfspace hs(Vec3f(1, 0, 0), 0); + Halfspace hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + Vec3s contact; + CoalScalar depth; + Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << -2.5, 0, 0; depth = -5; normal << -1, 0, 0; @@ -2145,14 +2144,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(-2.5, 0, 0)); + contact = transform.transform(Vec3s(-2.5, 0, 0)); depth = -5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(2.5, 0, 0)); contact << -1.25, 0, 0; depth = -7.5; normal << -1, 0, 0; @@ -2160,15 +2159,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); - contact = transform.transform(Vec3f(-1.25, 0, 0)); + tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); + contact = transform.transform(Vec3s(-1.25, 0, 0)); depth = -7.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-2.5, 0, 0)); contact << -3.75, 0, 0; depth = -2.5; normal << -1, 0, 0; @@ -2176,15 +2175,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); - contact = transform.transform(Vec3f(-3.75, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); + contact = transform.transform(Vec3s(-3.75, 0, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(5.1, 0, 0)); contact << 0.05, 0, 0; depth = -10.1; normal << -1, 0, 0; @@ -2192,27 +2191,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); - contact = transform.transform(Vec3f(0.05, 0, 0)); + tf2 = transform * Transform3s(Vec3s(5.1, 0, 0)); + contact = transform.transform(Vec3s(0.05, 0, 0)); depth = -10.1; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Halfspace(Vec3f(0, 1, 0), 0); + hs = Halfspace(Vec3s(0, 1, 0), 0); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, -2.5, 0; depth = -5; normal << 0, -1, 0; @@ -2221,14 +2220,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, -2.5, 0)); + contact = transform.transform(Vec3s(0, -2.5, 0)); depth = -5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 2.5, 0)); contact << 0, -1.25, 0; depth = -7.5; normal << 0, -1, 0; @@ -2236,15 +2235,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); - contact = transform.transform(Vec3f(0, -1.25, 0)); + tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); + contact = transform.transform(Vec3s(0, -1.25, 0)); depth = -7.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -2.5, 0)); contact << 0, -3.75, 0; depth = -2.5; normal << 0, -1, 0; @@ -2252,15 +2251,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); - contact = transform.transform(Vec3f(0, -3.75, 0)); + tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); + contact = transform.transform(Vec3s(0, -3.75, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 5.1, 0)); contact << 0, 0.05, 0; depth = -10.1; normal << 0, -1, 0; @@ -2268,27 +2267,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); - contact = transform.transform(Vec3f(0, 0.05, 0)); + tf2 = transform * Transform3s(Vec3s(0, 5.1, 0)); + contact = transform.transform(Vec3s(0, 0.05, 0)); depth = -10.1; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Halfspace(Vec3f(0, 0, 1), 0); + hs = Halfspace(Vec3s(0, 0, 1), 0); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, 0, -2.5; depth = -5; normal << 0, 0, -1; @@ -2297,14 +2296,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, -2.5)); + contact = transform.transform(Vec3s(0, 0, -2.5)); depth = -5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 2.5)); contact << 0, 0, -1.25; depth = -7.5; normal << 0, 0, -1; @@ -2312,15 +2311,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); - contact = transform.transform(Vec3f(0, 0, -1.25)); + tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); + contact = transform.transform(Vec3s(0, 0, -1.25)); depth = -7.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -2.5)); contact << 0, 0, -3.75; depth = -2.5; normal << 0, 0, -1; @@ -2328,15 +2327,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); - contact = transform.transform(Vec3f(0, 0, -3.75)); + tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); + contact = transform.transform(Vec3s(0, 0, -3.75)); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 5.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 5.1)); contact << 0, 0, 0.05; depth = -10.1; normal << 0, 0, -1; @@ -2344,42 +2343,42 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 5.1)); - contact = transform.transform(Vec3f(0, 0, 0.05)); + tf2 = transform * Transform3s(Vec3s(0, 0, 5.1)); + contact = transform.transform(Vec3s(0, 0, 0.05)); depth = -10.1; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -5.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -5.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -5.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, -5.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_planecylinder) { Cylinder s(5, 10); - Plane hs(Vec3f(1, 0, 0), 0); + Plane hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; - Vec3f p1, p2; + Vec3s contact; + CoalScalar depth; + Vec3s normal; + Vec3s p1, p2; - FCL_REAL eps = 1e-6; - tf1 = Transform3f(Vec3f(eps, 0, 0)); - tf2 = Transform3f(); + CoalScalar eps = 1e-6; + tf1 = Transform3s(Vec3s(eps, 0, 0)); + tf2 = Transform3s(); p1 << -5 + eps, 0, 0; p2 << 0, 0, 0; contact << (p1 + p2) / 2; @@ -2393,13 +2392,13 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; normal = - transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3f(eps, 0, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(eps, 0, 0)); + tf2 = Transform3s(); p1 << 5 + eps, 0, 0; p2 << 0, 0, 0; contact << (p1 + p2) / 2; @@ -2413,12 +2412,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; normal = - transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(2.5, 0, 0)); p1 << 5, 0, 0; p2 << 2.5, 0, 0; contact << (p1 + p2) / 2; @@ -2428,15 +2427,15 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(1, 0, 0); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-2.5, 0, 0)); p1 << -5, 0, 0; p2 << -2.5, 0, 0; contact << (p1 + p2) / 2; @@ -2446,38 +2445,38 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Plane(Vec3f(0, 1, 0), 0); + hs = Plane(Vec3s(0, 1, 0), 0); eps = 1e-6; - tf1 = Transform3f(Vec3f(0, eps, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(0, eps, 0)); + tf2 = Transform3s(); p1 << 0, -5 + eps, 0; p2 << 0, 0, 0; contact << (p1 + p2) / 2; @@ -2490,13 +2489,13 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; - normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3f(0, eps, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(0, eps, 0)); + tf2 = Transform3s(); p1 << 0, 5 + eps, 0; p2 << 0, 0, 0; contact << (p1 + p2) / 2; @@ -2509,12 +2508,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; - normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 2.5, 0)); p1 << 0, 5, 0; p2 << 0, 2.5, 0; contact << (p1 + p2) / 2; @@ -2524,15 +2523,15 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 1, 0); + normal = transform.getRotation() * Vec3s(0, 1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -2.5, 0)); p1 << 0, -5, 0; p2 << 0, -2.5, 0; contact << (p1 + p2) / 2; @@ -2542,38 +2541,38 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Plane(Vec3f(0, 0, 1), 0); + hs = Plane(Vec3s(0, 0, 1), 0); eps = 1e-6; - tf1 = Transform3f(Vec3f(0, 0, eps)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(0, 0, eps)); + tf2 = Transform3s(); p1 << 0, 0, -5 + eps; p2 << 0, 0, 0; contact << (p1 + p2) / 2; @@ -2586,13 +2585,13 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; - normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3f(0, 0, eps)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(0, 0, eps)); + tf2 = Transform3s(); p1 << 0, 0, 5 + eps; p2 << 0, 0, 0; contact << (p1 + p2) / 2; @@ -2605,12 +2604,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; - normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 2.5)); p1 << 0, 0, 5; p2 << 0, 0, 2.5; contact << (p1 + p2) / 2; @@ -2620,15 +2619,15 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 0, 1); + normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -2.5)); p1 << 0, 0, -5.; p2 << 0, 0, -2.5; contact << (p1 + p2) / 2; @@ -2638,50 +2637,50 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 10.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -10.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_halfspacecone) { Cone s(5, 10); - Halfspace hs(Vec3f(1, 0, 0), 0); + Halfspace hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + Vec3s contact; + CoalScalar depth; + Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << -2.5, 0, -5; depth = -5; normal << -1, 0, 0; @@ -2690,14 +2689,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(-2.5, 0, -5)); + contact = transform.transform(Vec3s(-2.5, 0, -5)); depth = -5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(2.5, 0, 0)); contact << -1.25, 0, -5; depth = -7.5; normal << -1, 0, 0; @@ -2705,15 +2704,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); - contact = transform.transform(Vec3f(-1.25, 0, -5)); + tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); + contact = transform.transform(Vec3s(-1.25, 0, -5)); depth = -7.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-2.5, 0, 0)); contact << -3.75, 0, -5; depth = -2.5; normal << -1, 0, 0; @@ -2721,15 +2720,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); - contact = transform.transform(Vec3f(-3.75, 0, -5)); + tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); + contact = transform.transform(Vec3s(-3.75, 0, -5)); depth = -2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(5.1, 0, 0)); contact << 0.05, 0, -5; depth = -10.1; normal << -1, 0, 0; @@ -2737,27 +2736,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); - contact = transform.transform(Vec3f(0.05, 0, -5)); + tf2 = transform * Transform3s(Vec3s(5.1, 0, 0)); + contact = transform.transform(Vec3s(0.05, 0, -5)); depth = -10.1; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Halfspace(Vec3f(0, 1, 0), 0); + hs = Halfspace(Vec3s(0, 1, 0), 0); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, -2.5, -5; depth = -5; normal << 0, -1, 0; @@ -2766,14 +2765,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, -2.5, -5)); + contact = transform.transform(Vec3s(0, -2.5, -5)); depth = -5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 2.5, 0)); contact << 0, -1.25, -5; depth = -7.5; normal << 0, -1, 0; @@ -2781,15 +2780,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); - contact = transform.transform(Vec3f(0, -1.25, -5)); + tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); + contact = transform.transform(Vec3s(0, -1.25, -5)); depth = -7.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -2.5, 0)); contact << 0, -3.75, -5; depth = -2.5; normal << 0, -1, 0; @@ -2797,15 +2796,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); - contact = transform.transform(Vec3f(0, -3.75, -5)); + tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); + contact = transform.transform(Vec3s(0, -3.75, -5)); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 5.1, 0)); contact << 0, 0.05, -5; depth = -10.1; normal << 0, -1, 0; @@ -2813,27 +2812,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); - contact = transform.transform(Vec3f(0, 0.05, -5)); + tf2 = transform * Transform3s(Vec3s(0, 5.1, 0)); + contact = transform.transform(Vec3s(0, 0.05, -5)); depth = -10.1; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Halfspace(Vec3f(0, 0, 1), 0); + hs = Halfspace(Vec3s(0, 0, 1), 0); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, 0, -2.5; depth = -5; normal << 0, 0, -1; @@ -2842,14 +2841,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, -2.5)); + contact = transform.transform(Vec3s(0, 0, -2.5)); depth = -5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 2.5)); contact << 0, 0, -1.25; depth = -7.5; normal << 0, 0, -1; @@ -2857,15 +2856,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); - contact = transform.transform(Vec3f(0, 0, -1.25)); + tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); + contact = transform.transform(Vec3s(0, 0, -1.25)); depth = -7.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -2.5)); contact << 0, 0, -3.75; depth = -2.5; normal << 0, 0, -1; @@ -2873,15 +2872,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); - contact = transform.transform(Vec3f(0, 0, -3.75)); + tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); + contact = transform.transform(Vec3s(0, 0, -3.75)); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 5.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 5.1)); contact << 0, 0, 0.05; depth = -10.1; normal << 0, 0, -1; @@ -2889,42 +2888,42 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 5.1)); - contact = transform.transform(Vec3f(0, 0, 0.05)); + tf2 = transform * Transform3s(Vec3s(0, 0, 5.1)); + contact = transform.transform(Vec3s(0, 0, 0.05)); depth = -10.1; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -5.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -5.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -5.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, -5.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_planecone) { Cone s(5, 10); - Plane hs(Vec3f(1, 0, 0), 0); + Plane hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; - Vec3f p1, p2; + Vec3s contact; + CoalScalar depth; + Vec3s normal; + Vec3s p1, p2; - FCL_REAL eps = 1e-6; - tf1 = Transform3f(Vec3f(eps, 0, 0)); - tf2 = Transform3f(); + CoalScalar eps = 1e-6; + tf1 = Transform3s(Vec3s(eps, 0, 0)); + tf2 = Transform3s(); p1 << -5 + eps, 0, -5; p2 << 0, 0, -5; contact << (p1 + p2) / 2; @@ -2938,13 +2937,13 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; normal = - transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3f(eps, 0, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(eps, 0, 0)); + tf2 = Transform3s(); p1 << 5 + eps, 0, -5; p2 << 0, 0, -5; contact << (p1 + p2) / 2; @@ -2958,12 +2957,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; normal = - transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(2.5, 0, 0)); p1 << 5, 0, -5; p2 << 2.5, 0, -5; contact << (p1 + p2) / 2; @@ -2973,15 +2972,15 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(1, 0, 0); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-2.5, 0, 0)); p1 << -5, 0, -5; p2 << -2.5, 0, -5; contact << (p1 + p2) / 2; @@ -2991,38 +2990,38 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Plane(Vec3f(0, 1, 0), 0); + hs = Plane(Vec3s(0, 1, 0), 0); eps = 1e-6; - tf1 = Transform3f(Vec3f(0, eps, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(0, eps, 0)); + tf2 = Transform3s(); p1 << 0, -5 + eps, -5; p2 << 0, 0, -5; contact << (p1 + p2) / 2; @@ -3035,13 +3034,13 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; - normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3f(0, eps, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(0, eps, 0)); + tf2 = Transform3s(); p1 << 0, 5 + eps, -5; p2 << 0, 0, -5; contact << (p1 + p2) / 2; @@ -3054,12 +3053,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; - normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 2.5, 0)); p1 << 0, 5, -5; p2 << 0, 2.5, -5; contact << (p1 + p2) / 2; @@ -3069,15 +3068,15 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 1, 0); + normal = transform.getRotation() * Vec3s(0, 1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -2.5, 0)); p1 << 0, -5, -5; p2 << 0, -2.5, -5; contact << (p1 + p2) / 2; @@ -3087,38 +3086,38 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Plane(Vec3f(0, 0, 1), 0); + hs = Plane(Vec3s(0, 0, 1), 0); eps = 1e-6; - tf1 = Transform3f(Vec3f(0, 0, eps)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(0, 0, eps)); + tf2 = Transform3s(); p1 << 0, 0, -5 + eps; p2 << 0, 0, 0; contact << (p1 + p2) / 2; @@ -3131,13 +3130,13 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; - normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3f(0, 0, eps)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(0, 0, eps)); + tf2 = Transform3s(); p1 << 0, 0, 5 + eps; p2 << 0, 0, 0; contact << (p1 + p2) / 2; @@ -3150,12 +3149,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; - normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 2.5)); p1 << 0, 0, 5; p2 << 0, 0, 2.5; contact << (p1 + p2) / 2; @@ -3165,15 +3164,15 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 0, 1); + normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -2.5)); p1 << 0, 0, -5; p2 << 0, 0, -2.5; contact << (p1 + p2) / 2; @@ -3183,48 +3182,48 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 10.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -10.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_planeplane) { - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Vec3f normal; - Vec3f contact; - FCL_REAL distance; + Vec3s normal; + Vec3s contact; + CoalScalar distance; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); { - Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset = 3.14; + Vec3s n = Vec3s::Random().normalized(); + CoalScalar offset = 3.14; Plane plane1(n, offset); Plane plane2(n, offset); @@ -3250,9 +3249,9 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) { } { - Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset1 = 3.14; - FCL_REAL offset2 = offset1 + 1.19841; + Vec3s n = Vec3s::Random().normalized(); + CoalScalar offset1 = 3.14; + CoalScalar offset2 = offset1 + 1.19841; Plane plane1(n, offset1); Plane plane2(n, offset2); @@ -3268,9 +3267,9 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) { } { - Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset1 = 3.14; - FCL_REAL offset2 = offset1 - 1.19841; + Vec3s n = Vec3s::Random().normalized(); + CoalScalar offset1 = 3.14; + CoalScalar offset2 = offset1 - 1.19841; Plane plane1(n, offset1); Plane plane2(n, offset2); @@ -3286,11 +3285,11 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) { } { - Vec3f n1(1, 0, 0); - FCL_REAL offset1 = 3.14; + Vec3s n1(1, 0, 0); + CoalScalar offset1 = 3.14; Plane plane1(n1, offset1); - Vec3f n2(0, 0, 1); - FCL_REAL offset2 = -2.13; + Vec3s n2(0, 0, 1); + CoalScalar offset2 = -2.13; Plane plane2(n2, offset2); tf1.setIdentity(); @@ -3308,11 +3307,11 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) { } { - Vec3f n1(1, 0, 0); - FCL_REAL offset1 = 3.14; + Vec3s n1(1, 0, 0); + CoalScalar offset1 = 3.14; Plane plane1(n1, offset1); - Vec3f n2(1, 1, 1); - FCL_REAL offset2 = -2.13; + Vec3s n2(1, 1, 1); + CoalScalar offset2 = -2.13; Plane plane2(n2, offset2); tf1.setIdentity(); @@ -3332,19 +3331,19 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) { } BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Vec3f normal; - Vec3f contact; - FCL_REAL distance; + Vec3s normal; + Vec3s contact; + CoalScalar distance; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); { - Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset = 3.14; + Vec3s n = Vec3s::Random().normalized(); + CoalScalar offset = 3.14; Halfspace hf1(n, offset); Halfspace hf2(n, offset); @@ -3362,9 +3361,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { } { - Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset1 = 3.14; - FCL_REAL offset2 = offset1 + 1.19841; + Vec3s n = Vec3s::Random().normalized(); + CoalScalar offset1 = 3.14; + CoalScalar offset2 = offset1 + 1.19841; Halfspace hf1(n, offset1); Halfspace hf2(n, offset2); @@ -3382,9 +3381,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { } { - Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset1 = 3.14; - FCL_REAL offset2 = offset1 - 1.19841; + Vec3s n = Vec3s::Random().normalized(); + CoalScalar offset1 = 3.14; + CoalScalar offset2 = offset1 - 1.19841; Halfspace hf1(n, offset1); Halfspace hf2(-n, -offset2); @@ -3403,11 +3402,11 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { } { - Vec3f n1(1, 0, 0); - FCL_REAL offset1 = 3.14; + Vec3s n1(1, 0, 0); + CoalScalar offset1 = 3.14; Halfspace hf1(n1, offset1); - Vec3f n2(0, 0, 1); - FCL_REAL offset2 = -2.13; + Vec3s n2(0, 0, 1); + CoalScalar offset2 = -2.13; Halfspace hf2(n2, offset2); tf1.setIdentity(); @@ -3424,11 +3423,11 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { } { - Vec3f n1(1, 0, 0); - FCL_REAL offset1 = 3.14; + Vec3s n1(1, 0, 0); + CoalScalar offset1 = 3.14; Halfspace hf1(n1, offset1); - Vec3f n2(1, 1, 1); - FCL_REAL offset2 = -2.13; + Vec3s n2(1, 1, 1); + CoalScalar offset2 = -2.13; Halfspace hf2(n2, offset2); tf1.setIdentity(); @@ -3448,19 +3447,19 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { } BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Vec3f normal; - Vec3f contact; - FCL_REAL distance; + Vec3s normal; + Vec3s contact; + CoalScalar distance; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); { - Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset = 3.14; + Vec3s n = Vec3s::Random().normalized(); + CoalScalar offset = 3.14; Halfspace hf(n, offset); Plane plane(n, offset); @@ -3479,9 +3478,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { } { - Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset1 = 3.14; - FCL_REAL offset2 = offset1 + 1.19841; + Vec3s n = Vec3s::Random().normalized(); + CoalScalar offset1 = 3.14; + CoalScalar offset2 = offset1 + 1.19841; Halfspace hf(n, offset1); Plane plane(n, offset2); @@ -3500,9 +3499,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { } { - Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset1 = 3.14; - FCL_REAL offset2 = offset1 - 1.19841; + Vec3s n = Vec3s::Random().normalized(); + CoalScalar offset1 = 3.14; + CoalScalar offset2 = offset1 - 1.19841; Halfspace hf(n, offset1); Plane plane(n, offset2); @@ -3521,11 +3520,11 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { } { - Vec3f n1(1, 0, 0); - FCL_REAL offset1 = 3.14; + Vec3s n1(1, 0, 0); + CoalScalar offset1 = 3.14; Halfspace hf(n1, offset1); - Vec3f n2(0, 0, 1); - FCL_REAL offset2 = -2.13; + Vec3s n2(0, 0, 1); + CoalScalar offset2 = -2.13; Plane plane(n2, offset2); tf1.setIdentity(); @@ -3542,11 +3541,11 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { } { - Vec3f n1(1, 0, 0); - FCL_REAL offset1 = 3.14; + Vec3s n1(1, 0, 0); + CoalScalar offset1 = 3.14; Halfspace hf(n1, offset1); - Vec3f n2(1, 1, 1); - FCL_REAL offset2 = -2.13; + Vec3s n2(1, 1, 1); + CoalScalar offset2 = -2.13; Plane plane(n2, offset2); tf1.setIdentity(); @@ -3568,70 +3567,70 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_spheresphere) { Sphere s1(20); Sphere s2(10); - Vec3f closest_p1, closest_p2, normal; + Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - FCL_REAL dist = -1; + CoalScalar dist = -1; dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration, + s1, Transform3s(), s2, Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(30.1, 0, 0)), + s1, Transform3s(), s2, Transform3s(Vec3s(30.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), + s1, Transform3s(), s2, Transform3s(Vec3s(29.9, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); - dist = solver1.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, - Transform3f(), compute_penetration, closest_p1, + dist = solver1.shapeDistance(s1, Transform3s(Vec3s(40, 0, 0)), s2, + Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); - dist = solver1.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2, - Transform3f(), compute_penetration, closest_p1, + dist = solver1.shapeDistance(s1, Transform3s(Vec3s(30.1, 0, 0)), s2, + Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); - dist = solver1.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2, - Transform3f(), compute_penetration, closest_p1, + dist = solver1.shapeDistance(s1, Transform3s(Vec3s(29.9, 0, 0)), s2, + Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist < 0); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(30.1, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(30.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(29.9, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist < 0); - dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2, + dist = solver1.shapeDistance(s1, transform * Transform3s(Vec3s(40, 0, 0)), s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); - dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), + dist = solver1.shapeDistance(s1, transform * Transform3s(Vec3s(30.1, 0, 0)), s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); - dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), + dist = solver1.shapeDistance(s1, transform * Transform3s(Vec3s(29.9, 0, 0)), s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist < 0); @@ -3640,15 +3639,15 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_spheresphere) { BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); - Vec3f closest_p1, closest_p2, normal; + Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - FCL_REAL dist; + CoalScalar dist; - dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), + dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); @@ -3659,57 +3658,57 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxbox) { BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), + s2, Transform3s(), s2, Transform3s(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)), + s2, Transform3s(), s2, Transform3s(Vec3s(20.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10.1) < 0.001); dist = solver1.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.2, 0)), + s2, Transform3s(), s2, Transform3s(Vec3s(0, 20.2, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10.2) < 0.001); dist = solver1.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)), + s2, Transform3s(), s2, Transform3s(Vec3s(10.1, 10.1, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001); dist = solver2.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), + s2, Transform3s(), s2, Transform3s(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver2.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)), + s2, Transform3s(), s2, Transform3s(Vec3s(20.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10.1) < 0.001); dist = solver2.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.1, 0)), + s2, Transform3s(), s2, Transform3s(Vec3s(0, 20.1, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10.1) < 0.001); dist = solver2.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)), + s2, Transform3s(), s2, Transform3s(Vec3s(10.1, 10.1, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(15.1, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(15.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(20, 0, 0)), compute_penetration, + s1, Transform3s(), s2, Transform3s(Vec3s(20, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 5) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(20, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(20, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 5) < 0.001); } @@ -3718,25 +3717,25 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylinderbox) { Cylinder s1(0.029, 0.1); Box s2(1.6, 0.6, 0.025); - Transform3f tf1( + Transform3s tf1( Quatf(0.5279170511703305, -0.50981118132505521, -0.67596178682051911, 0.0668715876735793), - Vec3f(0.041218354748013122, 1.2022554710435607, 0.77338855025700015)); + Vec3s(0.041218354748013122, 1.2022554710435607, 0.77338855025700015)); - Transform3f tf2( + Transform3s tf2( Quatf(0.70738826916719977, 0, 0, 0.70682518110536596), - Vec3f(-0.29936284351096382, 0.80023864435868775, 0.71750000000000003)); + Vec3s(-0.29936284351096382, 0.80023864435868775, 0.71750000000000003)); GJKSolver solver; - Vec3f p1, p2, normal; + Vec3s p1, p2, normal; bool compute_penetration = true; solver.shapeDistance(s1, tf1, s2, tf2, compute_penetration, p1, p2, normal); // If objects are not colliding, p2 should be outside the cylinder and // p1 should be outside the box - Vec3f p2Loc(tf1.inverse().transform(p2)); + Vec3s p2Loc(tf1.inverse().transform(p2)); bool p2_in_cylinder((fabs(p2Loc[2]) <= s1.halfLength) && (p2Loc[0] * p2Loc[0] + p2Loc[1] * p2Loc[1] <= s1.radius)); - Vec3f p1Loc(tf2.inverse().transform(p1)); + Vec3s p1Loc(tf2.inverse().transform(p1)); bool p1_in_box = (p1Loc.array().abs() <= s2.halfSide.array()).all(); std::cout << "p2 in cylinder = (" << p2Loc.transpose() << ")" << std::endl; std::cout << "p1 in box = (" << p1Loc.transpose() << ")" << std::endl; @@ -3760,7 +3759,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylinderbox) { s1 = Cylinder(0.06, 0.1); tf1.setTranslation( - Vec3f(-0.66734052046473924, 0.22219183277457269, 0.76825248755616293)); + Vec3s(-0.66734052046473924, 0.22219183277457269, 0.76825248755616293)); tf1.setQuatRotation(Quatf(0.52613359459338371, 0.32189408354839893, 0.70415587451837913, -0.35175580165512249)); solver.shapeDistance(s1, tf1, s2, tf2, compute_penetration, p1, p2, normal); @@ -3769,34 +3768,34 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylinderbox) { BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) { Sphere s1(20); Box s2(5, 5, 5); - Vec3f closest_p1, closest_p2, normal; + Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - FCL_REAL dist; + CoalScalar dist; int N = 10; for (int i = 0; i < N + 1; ++i) { - FCL_REAL dbox = 0.0001 + (s1.radius + s2.halfSide(0)) * i * 4 / (3 * N); - dist = solver1.shapeDistance(s1, Transform3f(Vec3f(dbox, 0., 0.)), s2, - Transform3f(), compute_penetration, closest_p1, + CoalScalar dbox = 0.0001 + (s1.radius + s2.halfSide(0)) * i * 4 / (3 * N); + dist = solver1.shapeDistance(s1, Transform3s(Vec3s(dbox, 0., 0.)), s2, + Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK_CLOSE(dist, (dbox - s1.radius - s2.halfSide(0)), 1e-6); - EIGEN_VECTOR_IS_APPROX(normal, -Vec3f(1, 0, 0), 1e-6); + EIGEN_VECTOR_IS_APPROX(normal, -Vec3s(1, 0, 0), 1e-6); dist = solver1.shapeDistance(s1, transform, s2, transform, compute_penetration, closest_p1, closest_p2, normal); dist = solver1.shapeDistance( - s1, transform * Transform3f(Vec3f(dbox, 0., 0.)), s2, transform, + s1, transform * Transform3s(Vec3s(dbox, 0., 0.)), s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK_CLOSE(dist, (dbox - s1.radius - s2.halfSide(0)), 1e-6); EIGEN_VECTOR_IS_APPROX(normal, -transform.getRotation().col(0), 1e-6); } - dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), + dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); @@ -3807,22 +3806,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) { BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)), + s1, Transform3s(), s2, Transform3s(Vec3s(22.6, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(22.6, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(22.6, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.01); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration, + s1, Transform3s(), s2, Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 17.5) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 17.5) < 0.001); } @@ -3830,19 +3829,19 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) { BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) { Cylinder s1(5, 10); Cylinder s2(5, 10); - Vec3f closest_p1, closest_p2, normal; + Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - FCL_REAL dist; + CoalScalar dist; { // The following situations corresponds to the case where the two cylinders // are exactly superposed. This is the worst case for EPA which will take // forever to converge with default parameters. - dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), + dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); @@ -3854,12 +3853,12 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) { // To handle the superposing case, we have to decrease the tolerance of EPA // and allow it to work with more vertices and faces. - FCL_REAL epa_tolerance_backup = solver1.epa_tolerance; + CoalScalar epa_tolerance_backup = solver1.epa_tolerance; size_t epa_max_iterations_backup = solver1.epa_max_iterations; solver1.epa_tolerance = 1e-2; solver1.epa_max_iterations = 1000; - dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), + dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); @@ -3875,22 +3874,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) { } dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), + s1, Transform3s(), s2, Transform3s(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration, + s1, Transform3s(), s2, Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.001); } @@ -3898,19 +3897,19 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) { BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) { Cone s1(5, 10); Cone s2(5, 10); - Vec3f closest_p1, closest_p2, normal; + Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - FCL_REAL dist; + CoalScalar dist; { // The following situations corresponds to the case where the two cones // are exactly superposed. This is the worst case for EPA which will take // forever to converge with default parameters. - dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), + dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); @@ -3922,12 +3921,12 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) { // To handle the superposing case, we have to decrease the tolerance of EPA // and allow it to work with more vertices and faces. - FCL_REAL epa_tolerance_backup = solver1.epa_tolerance; + CoalScalar epa_tolerance_backup = solver1.epa_tolerance; size_t epa_max_iterations_backup = solver1.epa_max_iterations; solver1.epa_tolerance = 1e-2; solver1.epa_max_iterations = 1000; - dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), + dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); @@ -3943,22 +3942,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) { } dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), + s1, Transform3s(), s2, Transform3s(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 40)), compute_penetration, + s1, Transform3s(), s2, Transform3s(Vec3s(0, 0, 40)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 1); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 40)), + s1, transform, s2, transform * Transform3s(Vec3s(0, 0, 40)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 1); } @@ -3966,19 +3965,19 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) { BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) { Cylinder s1(5, 10); Cone s2(5, 10); - Vec3f closest_p1, closest_p2, normal; + Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); - FCL_REAL dist; + CoalScalar dist; { // The following situations corresponds to the case where the two cones // are exactly superposed. This is the worst case for EPA which will take // forever to converge with default parameters. - dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), + dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); @@ -3990,12 +3989,12 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) { // To handle the superposing case, we have to decrease the tolerance of EPA // and allow it to work with more vertices and faces. - FCL_REAL epa_tolerance_backup = solver1.epa_tolerance; + CoalScalar epa_tolerance_backup = solver1.epa_tolerance; size_t epa_max_iterations_backup = solver1.epa_max_iterations; solver1.epa_tolerance = 1e-2; solver1.epa_max_iterations = 1000; - dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), + dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); @@ -4011,39 +4010,39 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) { } dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), + s1, Transform3s(), s2, Transform3s(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.01); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.02); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration, + s1, Transform3s(), s2, Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.01); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.1); } template void testReversibleShapeDistance(const S1& s1, const S2& s2, - FCL_REAL distance) { - Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0)); - Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0)); - - FCL_REAL distA; - FCL_REAL distB; - Vec3f p1A; - Vec3f p1B; - Vec3f p2A; - Vec3f p2B; - Vec3f normalA, normalB; + CoalScalar distance) { + Transform3s tf1(Vec3s(-0.5 * distance, 0.0, 0.0)); + Transform3s tf2(Vec3s(+0.5 * distance, 0.0, 0.0)); + + CoalScalar distA; + CoalScalar distB; + Vec3s p1A; + Vec3s p1B; + Vec3s p2A; + Vec3s p2B; + Vec3s normalA, normalB; bool compute_penetration = true; const double tol = 1e-6; @@ -4084,12 +4083,12 @@ BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes) { Capsule capsule(5, 10); Cone cone(5, 10); Cylinder cylinder(5, 10); - Plane plane(Vec3f(0, 0, 0), 0.0); - Halfspace halfspace(Vec3f(0, 0, 0), 0.0); + Plane plane(Vec3s(0, 0, 0), 0.0); + Halfspace halfspace(Vec3s(0, 0, 0), 0.0); // Use sufficiently long distance so that all the primitive shapes CANNOT // intersect - FCL_REAL distance = 15.0; + CoalScalar distance = 15.0; // If new shape distance algorithm is added for two distinct primitive // shapes, uncomment associated lines. For example, box-sphere intersection diff --git a/test/gjk.cpp b/test/gjk.cpp index d1877f475..9871029d5 100644 --- a/test/gjk.cpp +++ b/test/gjk.cpp @@ -34,31 +34,31 @@ /** \author Florent Lamiraux */ -#define BOOST_TEST_MODULE FCL_GJK +#define BOOST_TEST_MODULE COAL_GJK #include #include #include -#include -#include -#include -#include +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/internal/tools.h" +#include "coal/internal/shape_shape_func.h" #include "utility.h" -using hpp::fcl::FCL_REAL; -using hpp::fcl::GJKSolver; -using hpp::fcl::GJKVariant; -using hpp::fcl::Matrix3f; -using hpp::fcl::Quatf; -using hpp::fcl::Transform3f; -using hpp::fcl::TriangleP; -using hpp::fcl::Vec3f; +using coal::CoalScalar; +using coal::GJKSolver; +using coal::GJKVariant; +using coal::Matrix3s; +using coal::Quatf; +using coal::Transform3s; +using coal::TriangleP; +using coal::Vec3s; -typedef Eigen::Matrix vector_t; -typedef Eigen::Matrix vector6_t; -typedef Eigen::Matrix vector4_t; -typedef Eigen::Matrix matrix_t; +typedef Eigen::Matrix vector_t; +typedef Eigen::Matrix vector6_t; +typedef Eigen::Matrix vector4_t; +typedef Eigen::Matrix matrix_t; struct Result { bool collision; @@ -78,51 +78,51 @@ void test_gjk_distance_triangle_triangle( GJKSolver solver; if (enable_gjk_nesterov_acceleration) solver.gjk.gjk_variant = GJKVariant::NesterovAcceleration; - Transform3f tf1, tf2; - Vec3f p1, p2, a1, a2; - Matrix3f M; - FCL_REAL distance(sqrt(-1)); + Transform3s tf1, tf2; + Vec3s p1, p2, a1, a2; + Matrix3s M; + CoalScalar distance(sqrt(-1)); clock_t start, end; std::size_t nCol = 0, nDiff = 0; - FCL_REAL eps = 1e-7; + CoalScalar eps = 1e-7; Results_t results(N); for (std::size_t i = 0; i < N; ++i) { - Vec3f P1_loc(Vec3f::Random()), P2_loc(Vec3f::Random()), - P3_loc(Vec3f::Random()); - Vec3f Q1_loc(Vec3f::Random()), Q2_loc(Vec3f::Random()), - Q3_loc(Vec3f::Random()); + Vec3s P1_loc(Vec3s::Random()), P2_loc(Vec3s::Random()), + P3_loc(Vec3s::Random()); + Vec3s Q1_loc(Vec3s::Random()), Q2_loc(Vec3s::Random()), + Q3_loc(Vec3s::Random()); if (i == 0) { - P1_loc = Vec3f(0.063996093749999997, -0.15320971679687501, + P1_loc = Vec3s(0.063996093749999997, -0.15320971679687501, -0.42799999999999999); P2_loc = - Vec3f(0.069105957031249998, -0.150722900390625, -0.42999999999999999); - P3_loc = Vec3f(0.063996093749999997, -0.15320971679687501, + Vec3s(0.069105957031249998, -0.150722900390625, -0.42999999999999999); + P3_loc = Vec3s(0.063996093749999997, -0.15320971679687501, -0.42999999999999999); Q1_loc = - Vec3f(-25.655000000000001, -1.2858199462890625, 3.7249809570312502); - Q2_loc = Vec3f(-10.926, -1.284259033203125, 3.7281499023437501); - Q3_loc = Vec3f(-10.926, -1.2866180419921875, 3.72335400390625); - Transform3f tf( + Vec3s(-25.655000000000001, -1.2858199462890625, 3.7249809570312502); + Q2_loc = Vec3s(-10.926, -1.284259033203125, 3.7281499023437501); + Q3_loc = Vec3s(-10.926, -1.2866180419921875, 3.72335400390625); + Transform3s tf( Quatf(-0.42437287410898855, -0.26862477561450587, -0.46249645019513175, 0.73064726592483387), - Vec3f(-12.824601270753471, -1.6840516940066426, 3.8914453043793844)); + Vec3s(-12.824601270753471, -1.6840516940066426, 3.8914453043793844)); tf1 = tf; } else if (i == 1) { P1_loc = - Vec3f(-0.8027043342590332, -0.30276307463645935, -0.4372950792312622); + Vec3s(-0.8027043342590332, -0.30276307463645935, -0.4372950792312622); P2_loc = - Vec3f(-0.8027043342590332, 0.30276307463645935, -0.4372950792312622); + Vec3s(-0.8027043342590332, 0.30276307463645935, -0.4372950792312622); P3_loc = - Vec3f(0.8027043342590332, 0.30276307463645935, -0.4372950792312622); + Vec3s(0.8027043342590332, 0.30276307463645935, -0.4372950792312622); Q1_loc = - Vec3f(-0.224713996052742, -0.7417119741439819, 0.19999997317790985); + Vec3s(-0.224713996052742, -0.7417119741439819, 0.19999997317790985); Q2_loc = - Vec3f(-0.5247139930725098, -0.7417119741439819, 0.19999997317790985); + Vec3s(-0.5247139930725098, -0.7417119741439819, 0.19999997317790985); Q3_loc = - Vec3f(-0.224713996052742, -0.7417119741439819, 0.09999997168779373); - Matrix3f R; - Vec3f T; + Vec3s(-0.224713996052742, -0.7417119741439819, 0.09999997168779373); + Matrix3s R; + Vec3s T; R << 0.9657787025454787, 0.09400415350535746, 0.24173273843919627, -0.06713698817647556, 0.9908494114820345, -0.11709000206805695, -0.25052768814676646, 0.09685382227587608, 0.9632524147814993; @@ -131,21 +131,21 @@ void test_gjk_distance_triangle_triangle( tf1.setRotation(R); tf1.setTranslation(T); } else { - tf1 = Transform3f(); + tf1 = Transform3s(); } TriangleP tri1(P1_loc, P2_loc, P3_loc); TriangleP tri2(Q1_loc, Q2_loc, Q3_loc); - Vec3f normal; + Vec3s normal; const bool compute_penetration = true; - hpp::fcl::DistanceRequest request(compute_penetration, compute_penetration); - hpp::fcl::DistanceResult result; + coal::DistanceRequest request(compute_penetration, compute_penetration); + coal::DistanceResult result; start = clock(); // The specialized function TriangleP-TriangleP calls GJK to check for // collision and compute the witness points but it does not use EPA to // compute the penetration depth. - distance = hpp::fcl::ShapeShapeDistance( + distance = coal::ShapeShapeDistance( &tri1, tf1, &tri2, tf2, &solver, request, result); end = clock(); p1 = result.nearest_points[0]; @@ -155,15 +155,15 @@ void test_gjk_distance_triangle_triangle( results[i].timeGjk = end - start; results[i].collision = res; if (res) { - Vec3f c1, c2, normal2; + Vec3s c1, c2, normal2; ++nCol; // check that moving triangle 2 by the penetration depth in the // direction of the normal makes the triangles collision free. - FCL_REAL penetration_depth(-distance); + CoalScalar penetration_depth(-distance); assert(penetration_depth >= 0); tf2.setTranslation((penetration_depth + 10 - 4) * normal); result.clear(); - distance = hpp::fcl::ShapeShapeDistance( + distance = coal::ShapeShapeDistance( &tri1, tf1, &tri2, tf2, &solver, request, result); c1 = result.nearest_points[0]; c2 = result.nearest_points[1]; @@ -189,15 +189,15 @@ void test_gjk_distance_triangle_triangle( tf2.setIdentity(); } // Compute vectors between vertices - Vec3f P1(tf1.transform(P1_loc)), P2(tf1.transform(P2_loc)), + Vec3s P1(tf1.transform(P1_loc)), P2(tf1.transform(P2_loc)), P3(tf1.transform(P3_loc)), Q1(tf2.transform(Q1_loc)), Q2(tf2.transform(Q2_loc)), Q3(tf2.transform(Q3_loc)); - Vec3f u1(P2 - P1); - Vec3f v1(P3 - P1); - Vec3f w1(u1.cross(v1)); - Vec3f u2(Q2 - Q1); - Vec3f v2(Q3 - Q1); - Vec3f w2(u2.cross(v2)); + Vec3s u1(P2 - P1); + Vec3s v1(P3 - P1); + Vec3s w1(u1.cross(v1)); + Vec3s u2(Q2 - Q1); + Vec3s v2(Q3 - Q1); + Vec3s w2(u2.cross(v2)); BOOST_CHECK(w1.squaredNorm() > eps * eps); M.col(0) = u1; M.col(1) = v1; @@ -312,17 +312,17 @@ void test_gjk_distance_triangle_triangle( } std::cerr << "Total / average time gjk: " << totalTimeGjkNoColl + totalTimeGjkColl << ", " - << FCL_REAL(totalTimeGjkNoColl + totalTimeGjkColl) / - FCL_REAL(CLOCKS_PER_SEC * N) + << CoalScalar(totalTimeGjkNoColl + totalTimeGjkColl) / + CoalScalar(CLOCKS_PER_SEC * N) << "s" << std::endl; std::cerr << "-- Collisions -------------------------" << std::endl; std::cerr << "Total / average time gjk: " << totalTimeGjkColl << ", " - << FCL_REAL(totalTimeGjkColl) / FCL_REAL(CLOCKS_PER_SEC * nCol) + << CoalScalar(totalTimeGjkColl) / CoalScalar(CLOCKS_PER_SEC * nCol) << "s" << std::endl; std::cerr << "-- No collisions -------------------------" << std::endl; std::cerr << "Total / average time gjk: " << totalTimeGjkNoColl << ", " - << FCL_REAL(totalTimeGjkNoColl) / - FCL_REAL(CLOCKS_PER_SEC * (N - nCol)) + << CoalScalar(totalTimeGjkNoColl) / + CoalScalar(CLOCKS_PER_SEC * (N - nCol)) << "s" << std::endl; } @@ -334,17 +334,17 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_nesterov) { test_gjk_distance_triangle_triangle(true); } -void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray, +void test_gjk_unit_sphere(CoalScalar center_distance, Vec3s ray, double swept_sphere_radius, bool use_gjk_nesterov_acceleration) { - using namespace hpp::fcl; - const FCL_REAL r = 1.0; + using namespace coal; + const CoalScalar r = 1.0; Sphere sphere(r); sphere.setSweptSphereRadius(swept_sphere_radius); - typedef Eigen::Matrix Vec4f; - Transform3f tf0(Quatf(Vec4f::Random().normalized()), Vec3f::Zero()); - Transform3f tf1(Quatf(Vec4f::Random().normalized()), center_distance * ray); + typedef Eigen::Matrix Vec4f; + Transform3s tf0(Quatf(Vec4f::Random().normalized()), Vec3s::Zero()); + Transform3s tf1(Quatf(Vec4f::Random().normalized()), center_distance * ray); bool expect_collision = center_distance <= 2 * (r + swept_sphere_radius); @@ -359,7 +359,7 @@ void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray, details::GJK gjk(2, 1e-6); if (use_gjk_nesterov_acceleration) gjk.gjk_variant = GJKVariant::NesterovAcceleration; - details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0)); + details::GJK::Status status = gjk.evaluate(shape, Vec3s(1, 0, 0)); if (expect_collision) { BOOST_CHECK((status == details::GJK::Collision) || @@ -372,12 +372,12 @@ void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray, BOOST_CHECK_EQUAL(status, details::GJK::NoCollision); } - Vec3f w0, w1, normal; + Vec3s w0, w1, normal; gjk.getWitnessPointsAndNormal(shape, w0, w1, normal); - Vec3f w0_expected(tf0.inverse().transform(tf0.getTranslation() + ray) + + Vec3s w0_expected(tf0.inverse().transform(tf0.getTranslation() + ray) + swept_sphere_radius * normal); - Vec3f w1_expected(tf0.inverse().transform(tf1.getTranslation() - ray) - + Vec3s w1_expected(tf0.inverse().transform(tf1.getTranslation() - ray) - swept_sphere_radius * normal); EIGEN_VECTOR_IS_APPROX(w0, w0_expected, 1e-10); @@ -389,38 +389,38 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { std::array swept_sphere_radius = {0., 0.1, 1., 10., 100.}; for (bool nesterov_acceleration : use_nesterov_acceleration) { for (double ssr : swept_sphere_radius) { - test_gjk_unit_sphere(3, Vec3f(1, 0, 0), ssr, nesterov_acceleration); + test_gjk_unit_sphere(3, Vec3s(1, 0, 0), ssr, nesterov_acceleration); - test_gjk_unit_sphere(2.01, Vec3f(1, 0, 0), ssr, nesterov_acceleration); + test_gjk_unit_sphere(2.01, Vec3s(1, 0, 0), ssr, nesterov_acceleration); - test_gjk_unit_sphere(2.0, Vec3f(1, 0, 0), ssr, nesterov_acceleration); + test_gjk_unit_sphere(2.0, Vec3s(1, 0, 0), ssr, nesterov_acceleration); - test_gjk_unit_sphere(1.0, Vec3f(1, 0, 0), ssr, nesterov_acceleration); + test_gjk_unit_sphere(1.0, Vec3s(1, 0, 0), ssr, nesterov_acceleration); // Random rotation - test_gjk_unit_sphere(3, Vec3f::Random().normalized(), ssr, + test_gjk_unit_sphere(3, Vec3s::Random().normalized(), ssr, nesterov_acceleration); - test_gjk_unit_sphere(2.01, Vec3f::Random().normalized(), ssr, + test_gjk_unit_sphere(2.01, Vec3s::Random().normalized(), ssr, nesterov_acceleration); - test_gjk_unit_sphere(2.0, Vec3f::Random().normalized(), ssr, + test_gjk_unit_sphere(2.0, Vec3s::Random().normalized(), ssr, nesterov_acceleration); - test_gjk_unit_sphere(1.0, Vec3f::Random().normalized(), ssr, + test_gjk_unit_sphere(1.0, Vec3s::Random().normalized(), ssr, nesterov_acceleration); } } } -void test_gjk_triangle_capsule(Vec3f T, bool expect_collision, +void test_gjk_triangle_capsule(Vec3s T, bool expect_collision, bool use_gjk_nesterov_acceleration, - Vec3f w0_expected, Vec3f w1_expected) { - using namespace hpp::fcl; + Vec3s w0_expected, Vec3s w1_expected) { + using namespace coal; Capsule capsule(1., 2.); // Radius 1 and length 2 - TriangleP triangle(Vec3f(0., 0., 0.), Vec3f(1., 0., 0.), Vec3f(1., 1., 0.)); + TriangleP triangle(Vec3s(0., 0., 0.), Vec3s(1., 0., 0.), Vec3s(1., 1., 0.)); - Transform3f tf0, tf1; + Transform3s tf0, tf1; tf1.setTranslation(T); details::MinkowskiDiff shape; @@ -436,7 +436,7 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision, details::GJK gjk(10, 1e-6); if (use_gjk_nesterov_acceleration) gjk.gjk_variant = GJKVariant::NesterovAcceleration; - details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0)); + details::GJK::Status status = gjk.evaluate(shape, Vec3s(1, 0, 0)); if (expect_collision) { BOOST_CHECK((status == details::GJK::Collision) || @@ -445,19 +445,19 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision, BOOST_CHECK_EQUAL(status, details::GJK::NoCollision); // Check that guess works as expected - Vec3f guess = gjk.getGuessFromSimplex(); + Vec3s guess = gjk.getGuessFromSimplex(); details::GJK gjk2(3, 1e-6); details::GJK::Status status2 = gjk2.evaluate(shape, guess); BOOST_CHECK_EQUAL(status2, details::GJK::NoCollision); } - Vec3f w0, w1, normal; + Vec3s w0, w1, normal; if (status == details::GJK::NoCollision || status == details::GJK::CollisionWithPenetrationInformation) { gjk.getWitnessPointsAndNormal(shape, w0, w1, normal); } else { details::EPA epa(64, 1e-6); - details::EPA::Status epa_status = epa.evaluate(gjk, Vec3f(1, 0, 0)); + details::EPA::Status epa_status = epa.evaluate(gjk, Vec3s(1, 0, 0)); BOOST_CHECK_EQUAL(epa_status, details::EPA::AccuracyReached); epa.getWitnessPointsAndNormal(shape, w0, w1, normal); } @@ -468,23 +468,23 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision, BOOST_AUTO_TEST_CASE(triangle_capsule) { // GJK -> no collision - test_gjk_triangle_capsule(Vec3f(1.01, 0, 0), false, false, Vec3f(1., 0, 0), - Vec3f(0., 0, 0)); + test_gjk_triangle_capsule(Vec3s(1.01, 0, 0), false, false, Vec3s(1., 0, 0), + Vec3s(0., 0, 0)); // GJK + Nesterov acceleration -> no collision - test_gjk_triangle_capsule(Vec3f(1.01, 0, 0), false, true, Vec3f(1., 0, 0), - Vec3f(0., 0, 0)); + test_gjk_triangle_capsule(Vec3s(1.01, 0, 0), false, true, Vec3s(1., 0, 0), + Vec3s(0., 0, 0)); // GJK -> collision - test_gjk_triangle_capsule(Vec3f(0.5, 0, 0), true, false, Vec3f(1., 0, 0), - Vec3f(0., 0, 0)); + test_gjk_triangle_capsule(Vec3s(0.5, 0, 0), true, false, Vec3s(1., 0, 0), + Vec3s(0., 0, 0)); // GJK + Nesterov acceleration -> collision - test_gjk_triangle_capsule(Vec3f(0.5, 0, 0), true, true, Vec3f(1., 0, 0), - Vec3f(0., 0, 0)); + test_gjk_triangle_capsule(Vec3s(0.5, 0, 0), true, true, Vec3s(1., 0, 0), + Vec3s(0., 0, 0)); // GJK + EPA -> collision - test_gjk_triangle_capsule(Vec3f(-0.5, -0.01, 0), true, false, Vec3f(0, 1, 0), - Vec3f(0.5, 0, 0)); + test_gjk_triangle_capsule(Vec3s(-0.5, -0.01, 0), true, false, Vec3s(0, 1, 0), + Vec3s(0.5, 0, 0)); // GJK + Nesterov accleration + EPA -> collision - test_gjk_triangle_capsule(Vec3f(-0.5, -0.01, 0), true, true, Vec3f(0, 1, 0), - Vec3f(0.5, 0, 0)); + test_gjk_triangle_capsule(Vec3s(-0.5, -0.01, 0), true, true, Vec3s(0, 1, 0), + Vec3s(0.5, 0, 0)); } diff --git a/test/gjk_asserts.cpp b/test/gjk_asserts.cpp index b06ee09a7..c4cb0be98 100644 --- a/test/gjk_asserts.cpp +++ b/test/gjk_asserts.cpp @@ -1,20 +1,20 @@ -#define BOOST_TEST_MODULE FCL_GJK_ASSERTS +#define BOOST_TEST_MODULE COAL_GJK_ASSERTS #include #include -#include -#include +#include "coal/BVH/BVH_model.h" +#include "coal/collision.h" -using namespace hpp::fcl; +using namespace coal; -constexpr FCL_REAL pi = boost::math::constants::pi(); +constexpr CoalScalar pi = boost::math::constants::pi(); double DegToRad(const double& deg) { static double degToRad = pi / 180.; return deg * degToRad; } -std::vector dirs{Vec3f::UnitZ(), -Vec3f::UnitZ(), Vec3f::UnitY(), - -Vec3f::UnitY(), Vec3f::UnitX(), -Vec3f::UnitX()}; +std::vector dirs{Vec3s::UnitZ(), -Vec3s::UnitZ(), Vec3s::UnitY(), + -Vec3s::UnitY(), Vec3s::UnitX(), -Vec3s::UnitX()}; void CreateSphereMesh(BVHModel& model, const double& radius) { size_t polarSteps{32}; @@ -24,7 +24,7 @@ void CreateSphereMesh(BVHModel& model, const double& radius) { const float polarStep = PI / (float)(polarSteps - 1); const float azimuthStep = 2.0f * PI / (float)(azimuthSteps - 1); - std::vector vertices; + std::vector vertices; std::vector triangles; for (size_t p = 0; p < polarSteps; ++p) { @@ -64,8 +64,8 @@ BOOST_AUTO_TEST_CASE(TestSpheres) { ComputeCollision compute(&sphere2, &sphere1); - Transform3f sphere1Tf = Transform3f::Identity(); - Transform3f sphere2Tf = Transform3f::Identity(); + Transform3s sphere1Tf = Transform3s::Identity(); + Transform3s sphere2Tf = Transform3s::Identity(); for (int i = 0; i < 360; ++i) { for (int j = 0; j < 180; ++j) { @@ -77,9 +77,9 @@ BOOST_AUTO_TEST_CASE(TestSpheres) { (i == 86 && j == 52) || (i == 89 && j == 17) || (i == 89 && j == 58) || (i == 89 && j == 145)) { sphere2Tf.setQuatRotation( - Eigen::AngleAxis(DegToRad(i), Vec3f::UnitZ()) * - Eigen::AngleAxis(DegToRad(j), Vec3f::UnitY())); - for (const Vec3f& dir : dirs) { + Eigen::AngleAxis(DegToRad(i), Vec3s::UnitZ()) * + Eigen::AngleAxis(DegToRad(j), Vec3s::UnitY())); + for (const Vec3s& dir : dirs) { sphere2Tf.setTranslation(dir); CollisionResult result; diff --git a/test/gjk_convergence_criterion.cpp b/test/gjk_convergence_criterion.cpp index f2ac0bdbc..6002c38be 100644 --- a/test/gjk_convergence_criterion.cpp +++ b/test/gjk_convergence_criterion.cpp @@ -34,29 +34,29 @@ /** \author Louis Montaut */ -#include "hpp/fcl/data_types.h" -#include -#define BOOST_TEST_MODULE FCL_NESTEROV_GJK +#define BOOST_TEST_MODULE COAL_NESTEROV_GJK #include +#include #include -#include -#include -#include +#include "coal/data_types.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/internal/tools.h" #include "utility.h" -using hpp::fcl::Box; -using hpp::fcl::FCL_REAL; -using hpp::fcl::GJKConvergenceCriterion; -using hpp::fcl::GJKConvergenceCriterionType; -using hpp::fcl::GJKSolver; -using hpp::fcl::ShapeBase; -using hpp::fcl::support_func_guess_t; -using hpp::fcl::Transform3f; -using hpp::fcl::Vec3f; -using hpp::fcl::details::GJK; -using hpp::fcl::details::MinkowskiDiff; +using coal::Box; +using coal::CoalScalar; +using coal::GJKConvergenceCriterion; +using coal::GJKConvergenceCriterionType; +using coal::GJKSolver; +using coal::ShapeBase; +using coal::support_func_guess_t; +using coal::Transform3s; +using coal::Vec3s; +using coal::details::GJK; +using coal::details::MinkowskiDiff; using std::size_t; BOOST_AUTO_TEST_CASE(set_cv_criterion) { @@ -96,7 +96,7 @@ void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1, // by default GJK uses the VDB convergence criterion, which is relative. GJK gjk1(max_iterations, 1e-6); - FCL_REAL tol; + CoalScalar tol; switch (cv_type) { // need to lower the tolerance when absolute case GJKConvergenceCriterionType::Absolute: @@ -122,13 +122,13 @@ void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1, // Generate random transforms size_t n = 1000; - FCL_REAL extents[] = {-3., -3., 0, 3., 3., 3.}; - std::vector transforms; + CoalScalar extents[] = {-3., -3., 0, 3., 3., 3.}; + std::vector transforms; generateRandomTransforms(extents, transforms, n); - Transform3f identity = Transform3f::Identity(); + Transform3s identity = Transform3s::Identity(); // Same init for both solvers - Vec3f init_guess = Vec3f(1, 0, 0); + Vec3s init_guess = Vec3s(1, 0, 0); support_func_guess_t init_support_guess; init_support_guess.setZero(); @@ -138,21 +138,21 @@ void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1, GJK::Status res1 = gjk1.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(gjk1.getNumIterations() <= max_iterations); - Vec3f ray1 = gjk1.ray; + Vec3s ray1 = gjk1.ray; res1 = gjk1.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(res1 != GJK::Status::Failed); EIGEN_VECTOR_IS_APPROX(gjk1.ray, ray1, 1e-8); GJK::Status res2 = gjk2.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(gjk2.getNumIterations() <= max_iterations); - Vec3f ray2 = gjk2.ray; + Vec3s ray2 = gjk2.ray; res2 = gjk2.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(res2 != GJK::Status::Failed); EIGEN_VECTOR_IS_APPROX(gjk2.ray, ray2, 1e-8); GJK::Status res3 = gjk3.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(gjk3.getNumIterations() <= max_iterations); - Vec3f ray3 = gjk3.ray; + Vec3s ray3 = gjk3.ray; res3 = gjk3.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(res3 != GJK::Status::Failed); EIGEN_VECTOR_IS_APPROX(gjk3.ray, ray3, 1e-8); diff --git a/test/hfields.cpp b/test/hfields.cpp index ca34db96d..09d1b7449 100644 --- a/test/hfields.cpp +++ b/test/hfields.cpp @@ -34,45 +34,45 @@ /** \author Justin Carpentier */ -#define BOOST_TEST_MODULE FCL_HEIGHT_FIELDS +#define BOOST_TEST_MODULE COAL_HEIGHT_FIELDS #include #include #include "fcl_resources/config.h" -#include -#include -#include -#include -#include -#include +#include "coal/collision.h" +#include "coal/hfield.h" +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/mesh_loader/assimp.h" +#include "coal/mesh_loader/loader.h" -#include -#include +#include "coal/collision.h" +#include "coal/internal/traversal_node_hfield_shape.h" #include "utility.h" #include -using namespace hpp::fcl; +using namespace coal; template void test_constant_hfields(const Eigen::DenseIndex nx, const Eigen::DenseIndex ny, - const FCL_REAL min_altitude, - const FCL_REAL max_altitude) { - const FCL_REAL x_dim = 1., y_dim = 2.; - const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude); + const CoalScalar min_altitude, + const CoalScalar max_altitude) { + const CoalScalar x_dim = 1., y_dim = 2.; + const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude); HeightField hfield(x_dim, y_dim, heights, min_altitude); BOOST_CHECK(hfield.getXDim() == x_dim); BOOST_CHECK(hfield.getYDim() == y_dim); - const VecXf& x_grid = hfield.getXGrid(); + const VecXs& x_grid = hfield.getXGrid(); BOOST_CHECK(x_grid[0] == -x_dim / 2.); BOOST_CHECK(x_grid[nx - 1] == x_dim / 2.); - const VecXf& y_grid = hfield.getYGrid(); + const VecXs& y_grid = hfield.getYGrid(); BOOST_CHECK(y_grid[0] == y_dim / 2.); BOOST_CHECK(y_grid[ny - 1] == -y_dim / 2.); @@ -81,7 +81,7 @@ void test_constant_hfields(const Eigen::DenseIndex nx, for (Eigen::DenseIndex i = 0; i < nx; ++i) { for (Eigen::DenseIndex j = 0; j < ny; ++j) { - Vec3f point(x_grid[i], y_grid[j], heights(j, i)); + Vec3s point(x_grid[i], y_grid[j], heights(j, i)); BOOST_CHECK(hfield.aabb_local.contain(point)); } } @@ -97,24 +97,24 @@ void test_constant_hfields(const Eigen::DenseIndex nx, // Build equivalent object const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude); - const Transform3f box_placement( - Matrix3f::Identity(), Vec3f(0., 0., (max_altitude + min_altitude) / 2.)); + const Transform3s box_placement( + Matrix3s::Identity(), Vec3s(0., 0., (max_altitude + min_altitude) / 2.)); // Test collision const Sphere sphere(1.); - static const Transform3f IdTransform; + static const Transform3s IdTransform; - const Box box(Vec3f::Ones()); + const Box box(Vec3s::Ones()); - Transform3f M_sphere, M_box; + Transform3s M_sphere, M_box; // No collision case { - const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); + const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; CollisionResult result; @@ -137,11 +137,11 @@ void test_constant_hfields(const Eigen::DenseIndex nx, // Collision case { - const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude); + const CoalScalar eps_collision = -0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision)); CollisionRequest request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); @@ -165,15 +165,15 @@ void test_constant_hfields(const Eigen::DenseIndex nx, // Update height hfield.updateHeights( - MatrixXf::Constant(ny, nx, max_altitude / 2.)); // We change nothing + MatrixXs::Constant(ny, nx, max_altitude / 2.)); // We change nothing // No collision case { - const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); + const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; CollisionResult result; @@ -196,11 +196,11 @@ void test_constant_hfields(const Eigen::DenseIndex nx, // Collision case { - const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude); + const CoalScalar eps_collision = -0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision)); CollisionRequest request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); @@ -224,15 +224,15 @@ void test_constant_hfields(const Eigen::DenseIndex nx, // Restore height hfield.updateHeights( - MatrixXf::Constant(ny, nx, max_altitude)); // We change nothing + MatrixXs::Constant(ny, nx, max_altitude)); // We change nothing // Collision case { - const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude); + const CoalScalar eps_collision = -0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision)); CollisionRequest request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); @@ -256,7 +256,7 @@ void test_constant_hfields(const Eigen::DenseIndex nx, } BOOST_AUTO_TEST_CASE(building_constant_hfields) { - const FCL_REAL max_altitude = 1., min_altitude = 0.; + const CoalScalar max_altitude = 1., min_altitude = 0.; test_constant_hfields(2, 2, min_altitude, max_altitude); // Simple case @@ -272,33 +272,33 @@ BOOST_AUTO_TEST_CASE(building_constant_hfields) { template void test_negative_security_margin(const Eigen::DenseIndex nx, const Eigen::DenseIndex ny, - const FCL_REAL min_altitude, - const FCL_REAL max_altitude) { - const FCL_REAL x_dim = 1., y_dim = 2.; - const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude); + const CoalScalar min_altitude, + const CoalScalar max_altitude) { + const CoalScalar x_dim = 1., y_dim = 2.; + const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude); HeightField hfield(x_dim, y_dim, heights, min_altitude); // Build equivalent object const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude); - const Transform3f box_placement( - Matrix3f::Identity(), Vec3f(0., 0., (max_altitude + min_altitude) / 2.)); + const Transform3s box_placement( + Matrix3s::Identity(), Vec3s(0., 0., (max_altitude + min_altitude) / 2.)); // Test collision const Sphere sphere(1.); - static const Transform3f IdTransform; + static const Transform3s IdTransform; - const Box box(Vec3f::Ones()); + const Box box(Vec3s::Ones()); - Transform3f M_sphere, M_box; + Transform3s M_sphere, M_box; // No collision case { - const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); + const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; CollisionResult result; @@ -321,11 +321,11 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, // Collision case - positive security_margin { - const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); + const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; request.security_margin = eps_no_collision + 1e-6; @@ -349,11 +349,11 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, // Collision case { - const FCL_REAL eps_no_collision = -0.1 * (max_altitude - min_altitude); + const CoalScalar eps_no_collision = -0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; CollisionResult result; @@ -376,11 +376,11 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, // No collision case - negative security_margin { - const FCL_REAL eps_no_collision = -0.1 * (max_altitude - min_altitude); + const CoalScalar eps_no_collision = -0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; request.security_margin = eps_no_collision - 1e-4; @@ -404,7 +404,7 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, } BOOST_AUTO_TEST_CASE(negative_security_margin) { - const FCL_REAL max_altitude = 1., min_altitude = 0.; + const CoalScalar max_altitude = 1., min_altitude = 0.; // test_negative_security_margin(100, 100, min_altitude, // max_altitude); @@ -415,23 +415,23 @@ BOOST_AUTO_TEST_CASE(hfield_with_square_hole) { const Eigen::DenseIndex nx = 100, ny = 100; typedef AABB BV; - const MatrixXf X = + const MatrixXs X = Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1); - const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx); + const MatrixXs Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx); - const FCL_REAL dim_square = 0.5; + const CoalScalar dim_square = 0.5; const Eigen::Array hole = (X.array().abs() < dim_square) && (Y.array().abs() < dim_square); - const MatrixXf heights = - MatrixXf::Ones(ny, nx) - hole.cast().matrix(); + const MatrixXs heights = + MatrixXs::Ones(ny, nx) - hole.cast().matrix(); const HeightField hfield(2., 2., heights, -10.); Sphere sphere(0.48); - const Transform3f sphere_pos(Vec3f(0., 0., 0.5)); - const Transform3f hfield_pos; + const Transform3s sphere_pos(Vec3s(0., 0., 0.5)); + const Transform3s hfield_pos; const CollisionRequest request; @@ -459,17 +459,17 @@ BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) { // typedef OBBRSS BV; TODO(jcarpent): OBBRSS does not work (compile in Debug // mode), as the overlap of OBBRSS is not satisfactory yet. typedef AABB BV; - const MatrixXf X = + const MatrixXs X = Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1); - const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx); + const MatrixXs Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx); - const FCL_REAL dim_hole = 1; + const CoalScalar dim_hole = 1; const Eigen::Array hole = (X.array().square() + Y.array().square() <= dim_hole); - const MatrixXf heights = - MatrixXf::Ones(ny, nx) - hole.cast().matrix(); + const MatrixXs heights = + MatrixXs::Ones(ny, nx) - hole.cast().matrix(); const HeightField hfield(2., 2., heights, -10.); @@ -480,10 +480,10 @@ BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) { BOOST_CHECK(hfield.getYGrid()[ny - 1] == -1.); Sphere sphere(0.975); - const Transform3f sphere_pos(Vec3f(0., 0., 1.)); - const Transform3f hfield_pos; + const Transform3s sphere_pos(Vec3s(0., 0., 1.)); + const Transform3s hfield_pos; - const FCL_REAL thresholds[3] = {0., 0.01, -0.005}; + const CoalScalar thresholds[3] = {0., 0.01, -0.005}; for (int i = 0; i < 3; ++i) { CollisionResult result; @@ -515,24 +515,25 @@ BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) { } } -bool isApprox(const FCL_REAL v1, const FCL_REAL v2, const FCL_REAL tol = 1e-6) { +bool isApprox(const CoalScalar v1, const CoalScalar v2, + const CoalScalar tol = 1e-6) { return std::fabs(v1 - v2) <= tol; } -Vec3f computeFaceNormal(const Triangle& triangle, - const std::vector& points) { - const Vec3f pointA = points[triangle[0]]; - const Vec3f pointB = points[triangle[1]]; - const Vec3f pointC = points[triangle[2]]; +Vec3s computeFaceNormal(const Triangle& triangle, + const std::vector& points) { + const Vec3s pointA = points[triangle[0]]; + const Vec3s pointB = points[triangle[1]]; + const Vec3s pointC = points[triangle[2]]; return (pointB - pointA).cross(pointC - pointA).normalized(); } BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) { - const FCL_REAL sphere_radius = 1.; + const CoalScalar sphere_radius = 1.; Sphere sphere(sphere_radius); - MatrixXf altitutes(2, 2); - FCL_REAL altitude_value = 1.; + MatrixXs altitutes(2, 2); + CoalScalar altitude_value = 1.; altitutes.fill(altitude_value); typedef AABB BV; @@ -563,20 +564,20 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) { // Check face normals for convex1 { - const std::vector& points = *(convex1.points); + const std::vector& points = *(convex1.points); // BOTTOM { const Triangle& triangle = (*(convex1.polygons))[0]; BOOST_CHECK( - computeFaceNormal(triangle, points).isApprox(-Vec3f::UnitZ())); + computeFaceNormal(triangle, points).isApprox(-Vec3s::UnitZ())); } // TOP { const Triangle& triangle = (*(convex1.polygons))[1]; - BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3f::UnitZ())); + BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3s::UnitZ())); } // WEST sides @@ -585,14 +586,14 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) { const Triangle& triangle2 = (*(convex1.polygons))[3]; BOOST_CHECK( - computeFaceNormal(triangle1, points).isApprox(-Vec3f::UnitX())); + computeFaceNormal(triangle1, points).isApprox(-Vec3s::UnitX())); BOOST_CHECK( - computeFaceNormal(triangle2, points).isApprox(-Vec3f::UnitX())); + computeFaceNormal(triangle2, points).isApprox(-Vec3s::UnitX())); } // SOUTH-EAST sides { - const Vec3f south_east_normal = Vec3f(1., -1., 0).normalized(); + const Vec3s south_east_normal = Vec3s(1., -1., 0).normalized(); const Triangle& triangle1 = (*(convex1.polygons))[4]; const Triangle& triangle2 = (*(convex1.polygons))[5]; @@ -612,29 +613,29 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) { << computeFaceNormal(triangle1, points).transpose() << std::endl; BOOST_CHECK( - computeFaceNormal(triangle1, points).isApprox(Vec3f::UnitY())); + computeFaceNormal(triangle1, points).isApprox(Vec3s::UnitY())); BOOST_CHECK( - computeFaceNormal(triangle2, points).isApprox(Vec3f::UnitY())); + computeFaceNormal(triangle2, points).isApprox(Vec3s::UnitY())); } } // Check face normals for convex2 { - const std::vector& points = *(convex2.points); + const std::vector& points = *(convex2.points); // BOTTOM { const Triangle& triangle = (*(convex2.polygons))[0]; BOOST_CHECK( - computeFaceNormal(triangle, points).isApprox(-Vec3f::UnitZ())); + computeFaceNormal(triangle, points).isApprox(-Vec3s::UnitZ())); } // TOP { const Triangle& triangle = (*(convex2.polygons))[1]; - BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3f::UnitZ())); + BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3s::UnitZ())); } // SOUTH sides @@ -643,14 +644,14 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) { const Triangle& triangle2 = (*(convex2.polygons))[3]; BOOST_CHECK( - computeFaceNormal(triangle1, points).isApprox(-Vec3f::UnitY())); + computeFaceNormal(triangle1, points).isApprox(-Vec3s::UnitY())); BOOST_CHECK( - computeFaceNormal(triangle2, points).isApprox(-Vec3f::UnitY())); + computeFaceNormal(triangle2, points).isApprox(-Vec3s::UnitY())); } // NORTH-WEST sides { - const Vec3f north_west_normal = Vec3f(-1., 1., 0).normalized(); + const Vec3s north_west_normal = Vec3s(-1., 1., 0).normalized(); const Triangle& triangle1 = (*(convex2.polygons))[4]; const Triangle& triangle2 = (*(convex2.polygons))[5]; @@ -667,19 +668,19 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) { const Triangle& triangle2 = (*(convex2.polygons))[7]; BOOST_CHECK( - computeFaceNormal(triangle1, points).isApprox(Vec3f::UnitX())); + computeFaceNormal(triangle1, points).isApprox(Vec3s::UnitX())); BOOST_CHECK( - computeFaceNormal(triangle2, points).isApprox(Vec3f::UnitX())); + computeFaceNormal(triangle2, points).isApprox(Vec3s::UnitX())); } } } BOOST_AUTO_TEST_CASE(test_hfield_bin_active_faces) { typedef HFNodeBase::FaceOrientation FaceOrientation; - const FCL_REAL sphere_radius = 1.; + const CoalScalar sphere_radius = 1.; Sphere sphere(sphere_radius); - MatrixXf altitutes(3, 3); - FCL_REAL altitude_value = 1.; + MatrixXs altitutes(3, 3); + CoalScalar altitude_value = 1.; altitutes.fill(altitude_value); typedef AABB BV; @@ -713,10 +714,10 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_active_faces) { } BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { - const FCL_REAL sphere_radius = 1.; + const CoalScalar sphere_radius = 1.; Sphere sphere(sphere_radius); - MatrixXf altitutes(2, 2); - FCL_REAL altitude_value = 1.; + MatrixXs altitutes(2, 2); + CoalScalar altitude_value = 1.; altitutes.fill(altitude_value); typedef AABB BV; @@ -727,8 +728,8 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Collision from the TOP { - const Transform3f sphere_pos(Vec3f(0., 0., 2.)); - const Transform3f hfield_pos; + const Transform3s sphere_pos(Vec3s(0., 0., 2.)); + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -742,8 +743,8 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Same, but with a positive margin. { - const Transform3f sphere_pos(Vec3f(0., 0., 2.)); - const Transform3f hfield_pos; + const Transform3s sphere_pos(Vec3s(0., 0., 2.)); + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -753,7 +754,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { BOOST_CHECK(result.isCollision()); if (result.isCollision()) { const Contact& contact = result.getContact(0); - BOOST_CHECK(contact.normal.isApprox(Vec3f::UnitZ())); + BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitZ())); std::cout << "contact.penetration_depth: " << contact.penetration_depth << std::endl; BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); @@ -762,8 +763,8 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Collision from the BOTTOM { - const Transform3f sphere_pos(Vec3f(0., 0., -1.)); - const Transform3f hfield_pos; + const Transform3s sphere_pos(Vec3s(0., 0., -1.)); + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -774,8 +775,8 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { } { - const Transform3f sphere_pos(Vec3f(0., 0., -1.)); - const Transform3f hfield_pos; + const Transform3s sphere_pos(Vec3s(0., 0., -1.)); + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -785,7 +786,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { BOOST_CHECK(result.isCollision()); { const Contact& contact = result.getContact(0); - BOOST_CHECK(contact.normal.isApprox(-Vec3f::UnitZ())); + BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitZ())); std::cout << "contact.penetration_depth: " << contact.penetration_depth << std::endl; BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); @@ -794,9 +795,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Collision from the WEST { - const Transform3f sphere_pos( - Vec3f(hfield.getXGrid()[0] - sphere_radius, 0., 0.5)); - const Transform3f hfield_pos; + const Transform3s sphere_pos( + Vec3s(hfield.getXGrid()[0] - sphere_radius, 0., 0.5)); + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -807,9 +808,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { } { - const Transform3f sphere_pos( - Vec3f(hfield.getXGrid()[0] - sphere_radius, 0., 0.5)); - const Transform3f hfield_pos; + const Transform3s sphere_pos( + Vec3s(hfield.getXGrid()[0] - sphere_radius, 0., 0.5)); + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -819,16 +820,16 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { BOOST_CHECK(result.isCollision()); if (result.isCollision()) { const Contact& contact = result.getContact(0); - BOOST_CHECK(contact.normal.isApprox(-Vec3f::UnitX())); + BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitX())); BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); } } // Collision from the EAST { - const Transform3f sphere_pos( - Vec3f(hfield.getXGrid()[1] + sphere_radius, 0., 0.5)); - const Transform3f hfield_pos; + const Transform3s sphere_pos( + Vec3s(hfield.getXGrid()[1] + sphere_radius, 0., 0.5)); + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -839,9 +840,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { } { - const Transform3f sphere_pos( - Vec3f(hfield.getXGrid()[1] + sphere_radius, 0., 0.5)); - const Transform3f hfield_pos; + const Transform3s sphere_pos( + Vec3s(hfield.getXGrid()[1] + sphere_radius, 0., 0.5)); + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -852,16 +853,16 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { if (result.isCollision()) { const Contact& contact = result.getContact(0); - BOOST_CHECK(contact.normal.isApprox(Vec3f::UnitX())); + BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitX())); BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); } } // Collision from the NORTH { - const Transform3f sphere_pos( - Vec3f(0., hfield.getYGrid()[0] + sphere_radius, 0.5)); - const Transform3f hfield_pos; + const Transform3s sphere_pos( + Vec3s(0., hfield.getYGrid()[0] + sphere_radius, 0.5)); + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -872,9 +873,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { } { - const Transform3f sphere_pos( - Vec3f(0., hfield.getYGrid()[0] + sphere_radius, 0.5)); - const Transform3f hfield_pos; + const Transform3s sphere_pos( + Vec3s(0., hfield.getYGrid()[0] + sphere_radius, 0.5)); + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -885,16 +886,16 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { if (result.isCollision()) { const Contact& contact = result.getContact(0); - BOOST_CHECK(contact.normal.isApprox(Vec3f::UnitY())); + BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitY())); BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); } } // Collision from the SOUTH { - const Transform3f sphere_pos( - Vec3f(0., hfield.getYGrid()[1] - sphere_radius, 0.5)); - const Transform3f hfield_pos; + const Transform3s sphere_pos( + Vec3s(0., hfield.getYGrid()[1] - sphere_radius, 0.5)); + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -905,9 +906,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { } { - const Transform3f sphere_pos( - Vec3f(0., hfield.getYGrid()[1] - sphere_radius, 0.5)); - const Transform3f hfield_pos; + const Transform3s sphere_pos( + Vec3s(0., hfield.getYGrid()[1] - sphere_radius, 0.5)); + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -918,7 +919,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { if (result.isCollision()) { const Contact& contact = result.getContact(0); - BOOST_CHECK(contact.normal.isApprox(-Vec3f::UnitY())); + BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitY())); BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); } } diff --git a/test/math.cpp b/test/math.cpp index 3a5be3c9e..c0f16ff3e 100644 --- a/test/math.cpp +++ b/test/math.cpp @@ -36,25 +36,25 @@ #define _USE_MATH_DEFINES #include -#define BOOST_TEST_MODULE FCL_MATH +#define BOOST_TEST_MODULE COAL_MATH #include -#include -#include +#include "coal/data_types.h" +#include "coal/math/transform.h" -#include -#include +#include "coal/internal/intersect.h" +#include "coal/internal/tools.h" -using namespace hpp::fcl; +using namespace coal; BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64) { - Vec3f v1(1.0, 2.0, 3.0); + Vec3s v1(1.0, 2.0, 3.0); BOOST_CHECK(v1[0] == 1.0); BOOST_CHECK(v1[1] == 2.0); BOOST_CHECK(v1[2] == 3.0); - Vec3f v2 = v1; - Vec3f v3(3.3, 4.3, 5.3); + Vec3s v2 = v1; + Vec3s v3(3.3, 4.3, 5.3); v1 += v3; BOOST_CHECK(isEqual(v1, v2 + v3)); v1 -= v3; @@ -87,53 +87,53 @@ BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64) { BOOST_CHECK(isEqual(v1, (v2.array() - 2.0).matrix())); v1.array() += 2.0; - BOOST_CHECK(isEqual((-Vec3f(1.0, 2.0, 3.0)), Vec3f(-1.0, -2.0, -3.0))); + BOOST_CHECK(isEqual((-Vec3s(1.0, 2.0, 3.0)), Vec3s(-1.0, -2.0, -3.0))); - v1 = Vec3f(1.0, 2.0, 3.0); - v2 = Vec3f(3.0, 4.0, 5.0); - BOOST_CHECK(isEqual((v1.cross(v2)), Vec3f(-2.0, 4.0, -2.0))); + v1 = Vec3s(1.0, 2.0, 3.0); + v2 = Vec3s(3.0, 4.0, 5.0); + BOOST_CHECK(isEqual((v1.cross(v2)), Vec3s(-2.0, 4.0, -2.0))); BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5); - v1 = Vec3f(3.0, 4.0, 5.0); + v1 = Vec3s(3.0, 4.0, 5.0); BOOST_CHECK(std::abs(v1.squaredNorm() - 50) < 1e-5); BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5); BOOST_CHECK(isEqual(v1.normalized(), v1 / v1.norm())); - v1 = Vec3f(1.0, 2.0, 3.0); - v2 = Vec3f(3.0, 4.0, 5.0); - BOOST_CHECK(isEqual(v1.cross(v2), Vec3f(-2.0, 4.0, -2.0))); + v1 = Vec3s(1.0, 2.0, 3.0); + v2 = Vec3s(3.0, 4.0, 5.0); + BOOST_CHECK(isEqual(v1.cross(v2), Vec3s(-2.0, 4.0, -2.0))); BOOST_CHECK(v1.dot(v2) == 26); } -Vec3f rotate(Vec3f input, FCL_REAL w, Vec3f vec) { +Vec3s rotate(Vec3s input, CoalScalar w, Vec3s vec) { return 2 * vec.dot(input) * vec + (w * w - vec.dot(vec)) * input + 2 * w * vec.cross(input); } BOOST_AUTO_TEST_CASE(quaternion) { Quatf q1(Quatf::Identity()), q2, q3; - q2 = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2); + q2 = fromAxisAngle(Vec3s(0, 0, 1), M_PI / 2); q3 = q2.inverse(); - Vec3f v(1, -1, 0); + Vec3s v(1, -1, 0); BOOST_CHECK(isEqual(v, q1 * v)); - BOOST_CHECK(isEqual(Vec3f(1, 1, 0), q2 * v)); + BOOST_CHECK(isEqual(Vec3s(1, 1, 0), q2 * v)); BOOST_CHECK( - isEqual(rotate(v, q3.w(), Vec3f(q3.x(), q3.y(), q3.z())), q3 * v)); + isEqual(rotate(v, q3.w(), Vec3s(q3.x(), q3.y(), q3.z())), q3 * v)); } BOOST_AUTO_TEST_CASE(transform) { - Quatf q = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2); - Vec3f T(0, 1, 2); - Transform3f tf(q, T); + Quatf q = fromAxisAngle(Vec3s(0, 0, 1), M_PI / 2); + Vec3s T(0, 1, 2); + Transform3s tf(q, T); - Vec3f v(1, -1, 0); + Vec3s v(1, -1, 0); BOOST_CHECK(isEqual(q * v + T, q * v + T)); - Vec3f rv(q * v); - // typename Transform3f::transform_return_type::type output = + Vec3s rv(q * v); + // typename Transform3s::transform_return_type::type output = // tf * v; // std::cout << rv << std::endl; // std::cout << output.lhs() << std::endl; @@ -142,7 +142,7 @@ BOOST_AUTO_TEST_CASE(transform) { BOOST_AUTO_TEST_CASE(random_transform) { for (size_t i = 0; i < 100; ++i) { - const Transform3f tf = Transform3f::Random(); + const Transform3s tf = Transform3s::Random(); BOOST_CHECK((tf.inverseTimes(tf)).isIdentity()); } } diff --git a/test/normal_and_nearest_points.cpp b/test/normal_and_nearest_points.cpp index a3ef2a696..322777480 100644 --- a/test/normal_and_nearest_points.cpp +++ b/test/normal_and_nearest_points.cpp @@ -34,19 +34,19 @@ /** \author Louis Montaut */ -#define BOOST_TEST_MODULE NORMAL_AND_NEAREST_POINTS +#define BOOST_TEST_MODULE COAL_NORMAL_AND_NEAREST_POINTS #include -#include -#include -#include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/shape/geometric_shapes.h" +#include "coal/collision_data.h" +#include "coal/BV/OBBRSS.h" +#include "coal/BVH/BVH_model.h" +#include "coal/shape/geometric_shape_to_BVH_model.h" #include "utility.h" -using namespace hpp::fcl; +using namespace coal; typedef Eigen::Vector2d Vec2d; // This test suite is designed to operate on any pair of primitive shapes: @@ -75,9 +75,9 @@ template void test_normal_and_nearest_points( const ShapeType1& o1, const ShapeType2& o2, size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS, - FCL_REAL gjk_tolerance = GJK_DEFAULT_TOLERANCE, + CoalScalar gjk_tolerance = GJK_DEFAULT_TOLERANCE, size_t epa_max_iterations = EPA_DEFAULT_MAX_ITERATIONS, - FCL_REAL epa_tolerance = EPA_DEFAULT_TOLERANCE) { + CoalScalar epa_tolerance = EPA_DEFAULT_TOLERANCE) { // Generate random poses for o2 #ifndef NDEBUG // if debug mode std::size_t n = 10; @@ -86,13 +86,13 @@ void test_normal_and_nearest_points( #endif // We want to make sure we generate poses that are in collision // so we take a relatively small extent for the random poses - FCL_REAL extents[] = {-1.5, -1.5, -1.5, 1.5, 1.5, 1.5}; - std::vector transforms; + CoalScalar extents[] = {-1.5, -1.5, -1.5, 1.5, 1.5, 1.5}; + std::vector transforms; generateRandomTransforms(extents, transforms, n); - Transform3f tf1 = Transform3f::Identity(); + Transform3s tf1 = Transform3s::Identity(); CollisionRequest colreq; - colreq.distance_upper_bound = std::numeric_limits::max(); + colreq.distance_upper_bound = std::numeric_limits::max(); // For strictly convex shapes, the default tolerance of EPA is way too low. // Because EPA is basically trying to fit a polytope to a strictly convex // surface, it might take it a lot of iterations to converge to a low @@ -114,30 +114,30 @@ void test_normal_and_nearest_points( // Since CollisionRequest::distance_lower_bound is set to infinity, // these functions should agree on the results regardless of collision or // not. - Transform3f tf2 = transforms[i]; + Transform3s tf2 = transforms[i]; CollisionResult colres; DistanceResult distres; size_t col = collide(&o1, tf1, &o2, tf2, colreq, colres); - FCL_REAL dist = distance(&o1, tf1, &o2, tf2, distreq, distres); + CoalScalar dist = distance(&o1, tf1, &o2, tf2, distreq, distres); - const FCL_REAL dummy_precision(100 * - std::numeric_limits::epsilon()); + const CoalScalar dummy_precision( + 100 * std::numeric_limits::epsilon()); if (col) { BOOST_CHECK(dist <= 0.); BOOST_CHECK_CLOSE(dist, distres.min_distance, dummy_precision); Contact contact = colres.getContact(0); BOOST_CHECK_CLOSE(dist, contact.penetration_depth, dummy_precision); - Vec3f cp1 = contact.nearest_points[0]; + Vec3s cp1 = contact.nearest_points[0]; EIGEN_VECTOR_IS_APPROX(cp1, distres.nearest_points[0], dummy_precision); - Vec3f cp2 = contact.nearest_points[1]; + Vec3s cp2 = contact.nearest_points[1]; EIGEN_VECTOR_IS_APPROX(cp2, distres.nearest_points[1], dummy_precision); BOOST_CHECK_CLOSE(contact.penetration_depth, -(cp2 - cp1).norm(), epa_tolerance); EIGEN_VECTOR_IS_APPROX(cp1, cp2 - dist * distres.normal, epa_tolerance); - Vec3f separation_vector = contact.penetration_depth * contact.normal; + Vec3s separation_vector = contact.penetration_depth * contact.normal; EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, epa_tolerance); if (dist < 0) { @@ -146,27 +146,27 @@ void test_normal_and_nearest_points( } // Separate the shapes - Transform3f new_tf1 = tf1; - FCL_REAL eps = 1e-2; + Transform3s new_tf1 = tf1; + CoalScalar eps = 1e-2; new_tf1.setTranslation(tf1.getTranslation() + separation_vector - eps * contact.normal); CollisionResult new_colres; DistanceResult new_distres; size_t new_col = collide(&o1, new_tf1, &o2, tf2, colreq, new_colres); - FCL_REAL new_dist = + CoalScalar new_dist = distance(&o1, new_tf1, &o2, tf2, distreq, new_distres); BOOST_CHECK(new_dist > 0); BOOST_CHECK(!new_col); BOOST_CHECK(!new_colres.isCollision()); BOOST_CHECK_CLOSE(new_colres.distance_lower_bound, new_dist, epa_tolerance); - Vec3f new_cp1 = new_distres.nearest_points[0]; - Vec3f new_cp2 = new_distres.nearest_points[1]; + Vec3s new_cp1 = new_distres.nearest_points[0]; + Vec3s new_cp2 = new_distres.nearest_points[1]; BOOST_CHECK_CLOSE(new_dist, (new_cp1 - new_cp2).norm(), epa_tolerance); EIGEN_VECTOR_IS_APPROX(new_cp1, new_cp2 - new_dist * new_distres.normal, epa_tolerance); - Vec3f new_separation_vector = new_dist * new_distres.normal; + Vec3s new_separation_vector = new_dist * new_distres.normal; EIGEN_VECTOR_IS_APPROX(new_separation_vector, new_cp2 - new_cp1, epa_tolerance); @@ -179,12 +179,12 @@ void test_normal_and_nearest_points( BOOST_CHECK_CLOSE(distres.min_distance, dist, dummy_precision); BOOST_CHECK_CLOSE(dist, colres.distance_lower_bound, dummy_precision); - Vec3f cp1 = distres.nearest_points[0]; - Vec3f cp2 = distres.nearest_points[1]; + Vec3s cp1 = distres.nearest_points[0]; + Vec3s cp2 = distres.nearest_points[1]; BOOST_CHECK_CLOSE(dist, (cp1 - cp2).norm(), gjk_tolerance); EIGEN_VECTOR_IS_APPROX(cp1, cp2 - dist * distres.normal, gjk_tolerance); - Vec3f separation_vector = dist * distres.normal; + Vec3s separation_vector = dist * distres.normal; EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, gjk_tolerance); if (dist > 0) { @@ -199,14 +199,14 @@ void test_normal_and_nearest_points( // If you translate one of the cones by the separation vector and it // happens to be parallel to the axis of the cone, the two shapes will // still be disjoint. - FCL_REAL eps = 1e-2; - Transform3f new_tf1 = tf1; + CoalScalar eps = 1e-2; + Transform3s new_tf1 = tf1; new_tf1.setTranslation(tf1.getTranslation() + separation_vector + eps * distres.normal); CollisionResult new_colres; DistanceResult new_distres; collide(&o1, new_tf1, &o2, tf2, colreq, new_colres); - FCL_REAL new_dist = + CoalScalar new_dist = distance(&o1, new_tf1, &o2, tf2, distreq, new_distres); BOOST_CHECK(new_dist < dist); BOOST_CHECK_CLOSE(new_colres.distance_lower_bound, new_dist, @@ -214,11 +214,11 @@ void test_normal_and_nearest_points( // tolerance if (new_colres.isCollision()) { Contact contact = new_colres.getContact(0); - Vec3f new_cp1 = contact.nearest_points[0]; + Vec3s new_cp1 = contact.nearest_points[0]; EIGEN_VECTOR_IS_APPROX(new_cp1, new_distres.nearest_points[0], dummy_precision); - Vec3f new_cp2 = contact.nearest_points[1]; + Vec3s new_cp2 = contact.nearest_points[1]; EIGEN_VECTOR_IS_APPROX(new_cp2, new_distres.nearest_points[1], dummy_precision); BOOST_CHECK_CLOSE(contact.penetration_depth, @@ -226,7 +226,7 @@ void test_normal_and_nearest_points( EIGEN_VECTOR_IS_APPROX(new_cp1, new_cp2 - new_dist * new_distres.normal, epa_tolerance); - Vec3f new_separation_vector = + Vec3s new_separation_vector = contact.penetration_depth * contact.normal; EIGEN_VECTOR_IS_APPROX(new_separation_vector, new_cp2 - new_cp1, epa_tolerance); @@ -241,17 +241,18 @@ void test_normal_and_nearest_points( } template -Eigen::Matrix generateRandomVector(FCL_REAL min, - FCL_REAL max) { - typedef Eigen::Matrix VecType; +Eigen::Matrix generateRandomVector(CoalScalar min, + CoalScalar max) { + typedef Eigen::Matrix VecType; // Generate a random vector in the [min, max] range VecType v = VecType::Random() * (max - min) * 0.5 + VecType::Ones() * (max + min) * 0.5; return v; } -FCL_REAL generateRandomNumber(FCL_REAL min, FCL_REAL max) { - FCL_REAL r = static_cast(rand()) / static_cast(RAND_MAX); +CoalScalar generateRandomNumber(CoalScalar min, CoalScalar max) { + CoalScalar r = + static_cast(rand()) / static_cast(RAND_MAX); r = 2 * r - 1.0; return r * (max - min) * 0.5 + (max + min) * 0.5; } @@ -269,7 +270,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_sphere) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_capsule) { for (size_t i = 0; i < 10; ++i) { Vec2d radii = generateRandomVector<2>(0.05, 1.0); - FCL_REAL h = generateRandomNumber(0.15, 1.0); + CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Sphere(radii(0))); shared_ptr o2(new Capsule(radii(1), h)); @@ -300,9 +301,9 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_mesh_mesh) { o2_.points, o2_.num_points, o2_.polygons, o2_.num_polygons)); size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS; - FCL_REAL gjk_tolerance = GJK_DEFAULT_TOLERANCE; + CoalScalar gjk_tolerance = GJK_DEFAULT_TOLERANCE; size_t epa_max_iterations = EPA_DEFAULT_MAX_ITERATIONS; - FCL_REAL epa_tolerance = EPA_DEFAULT_TOLERANCE; + CoalScalar epa_tolerance = EPA_DEFAULT_TOLERANCE; test_normal_and_nearest_points(*o1.get(), *o2.get(), gjk_max_iterations, gjk_tolerance, epa_max_iterations, epa_tolerance); @@ -328,12 +329,12 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_mesh_ellipsoid) { o1_.points, o1_.num_points, o1_.polygons, o1_.num_polygons)); shared_ptr o2(new Ellipsoid(generateRandomVector<3>(0.05, 1.0))); - FCL_REAL gjk_tolerance = 1e-6; + CoalScalar gjk_tolerance = 1e-6; // With EPA's tolerance set at 1e-3, the precision on the normal, contact // points and penetration depth is on the order of the milimeter. However, // EPA (currently) cannot converge to lower tolerances on strictly convex // shapes in a reasonable amount of iterations. - FCL_REAL epa_tolerance = 1e-3; + CoalScalar epa_tolerance = 1e-3; test_normal_and_nearest_points(*o1.get(), *o2.get(), GJK_DEFAULT_MAX_ITERATIONS, gjk_tolerance, EPA_DEFAULT_MAX_ITERATIONS, epa_tolerance); @@ -349,13 +350,13 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_ellipsoid) { shared_ptr o2(new Ellipsoid(generateRandomVector<3>(0.05, 1.0))); size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS; - FCL_REAL gjk_tolerance = 1e-6; + CoalScalar gjk_tolerance = 1e-6; // With EPA's tolerance set at 1e-3, the precision on the normal, contact // points and penetration depth is on the order of the milimeter. However, // EPA (currently) cannot converge to lower tolerances on strictly convex // shapes in a reasonable amount of iterations. size_t epa_max_iterations = 250; - FCL_REAL epa_tolerance = 1e-3; + CoalScalar epa_tolerance = 1e-3; // For EPA on ellipsoids, we need to increase the number of iterations in // this test. This is simply because this test checks **a lot** of cases and // it can generate some of the worst cases for EPA. We don't want to @@ -370,8 +371,8 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_ellipsoid) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_plane) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Box(generateRandomVector<3>(0.05, 1.0))); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); @@ -383,8 +384,8 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_plane) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_halfspace) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Box(generateRandomVector<3>(0.05, 1.0))); - FCL_REAL offset = 0.1; - Vec3f n = Vec3f::Random(); + CoalScalar offset = 0.1; + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -395,11 +396,11 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_halfspace) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_halfspace) { for (size_t i = 0; i < 10; ++i) { - FCL_REAL r = generateRandomNumber(0.05, 1.0); - FCL_REAL h = generateRandomNumber(0.15, 1.0); + CoalScalar r = generateRandomNumber(0.05, 1.0); + CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Capsule(r, h)); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -411,8 +412,8 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_halfspace) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_halfspace) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Sphere(generateRandomNumber(0.05, 1.0))); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -424,8 +425,8 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_halfspace) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_plane) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Sphere(generateRandomNumber(0.05, 1.0))); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); @@ -440,8 +441,8 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_mesh_halfspace) { Ellipsoid(generateRandomVector<3>(0.05, 1.0))); shared_ptr> o1(new Convex( o1_.points, o1_.num_points, o1_.polygons, o1_.num_polygons)); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -458,9 +459,9 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_cylinder) { shared_ptr o2(new Cylinder(r(1), h(1))); size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS; - FCL_REAL gjk_tolerance = 1e-6; + CoalScalar gjk_tolerance = 1e-6; size_t epa_max_iterations = 250; - FCL_REAL epa_tolerance = 1e-3; + CoalScalar epa_tolerance = 1e-3; test_normal_and_nearest_points(*o1.get(), *o2.get(), gjk_max_iterations, gjk_tolerance, epa_max_iterations, epa_tolerance); @@ -478,9 +479,9 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_ellipsoid) { shared_ptr o2(new Cylinder(r(1), h(1))); size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS; - FCL_REAL gjk_tolerance = 1e-6; + CoalScalar gjk_tolerance = 1e-6; size_t epa_max_iterations = 250; - FCL_REAL epa_tolerance = 1e-3; + CoalScalar epa_tolerance = 1e-3; test_normal_and_nearest_points(*o1.get(), *o2.get(), gjk_max_iterations, gjk_tolerance, epa_max_iterations, epa_tolerance); @@ -492,11 +493,11 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_ellipsoid) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_halfspace) { for (size_t i = 0; i < 10; ++i) { - FCL_REAL r = generateRandomNumber(0.05, 1.0); - FCL_REAL h = generateRandomNumber(0.15, 1.0); + CoalScalar r = generateRandomNumber(0.05, 1.0); + CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Cone(r, h)); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -507,11 +508,11 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_halfspace) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_halfspace) { for (size_t i = 0; i < 10; ++i) { - FCL_REAL r = generateRandomNumber(0.05, 1.0); - FCL_REAL h = generateRandomNumber(0.15, 1.0); + CoalScalar r = generateRandomNumber(0.05, 1.0); + CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Cylinder(r, h)); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -522,11 +523,11 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_halfspace) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_plane) { for (size_t i = 0; i < 10; ++i) { - FCL_REAL r = generateRandomNumber(0.05, 1.0); - FCL_REAL h = generateRandomNumber(0.15, 1.0); + CoalScalar r = generateRandomNumber(0.05, 1.0); + CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Cone(r, h)); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); @@ -537,11 +538,11 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_plane) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_plane) { for (size_t i = 0; i < 10; ++i) { - FCL_REAL r = generateRandomNumber(0.05, 1.0); - FCL_REAL h = generateRandomNumber(0.15, 1.0); + CoalScalar r = generateRandomNumber(0.05, 1.0); + CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Cylinder(r, h)); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); @@ -552,11 +553,11 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_plane) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_plane) { for (size_t i = 0; i < 10; ++i) { - FCL_REAL r = generateRandomNumber(0.05, 1.0); - FCL_REAL h = generateRandomNumber(0.15, 1.0); + CoalScalar r = generateRandomNumber(0.05, 1.0); + CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Capsule(r, h)); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); @@ -580,7 +581,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_capsule) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_cylinder) { for (size_t i = 0; i < 10; ++i) { Vec2d r = generateRandomVector<2>(0.05, 1.0); - FCL_REAL h = generateRandomNumber(0.15, 1.0); + CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Sphere(r(0))); shared_ptr o2(new Cylinder(r(1), h)); @@ -591,8 +592,8 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_cylinder) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_halfspace) { for (size_t i = 0; i < 10; ++i) { - FCL_REAL offset = generateRandomNumber(0.15, 1.0); - Vec3f n = Vec3f::Random(); + CoalScalar offset = generateRandomNumber(0.15, 1.0); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o1(new Ellipsoid(generateRandomVector<3>(0.05, 1.0))); shared_ptr o2(new Halfspace(n, offset)); @@ -604,8 +605,8 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_halfspace) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_plane) { for (size_t i = 0; i < 10; ++i) { - FCL_REAL offset = generateRandomNumber(0.15, 1.0); - Vec3f n = Vec3f::Random(); + CoalScalar offset = generateRandomNumber(0.15, 1.0); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o1(new Ellipsoid(generateRandomVector<3>(0.05, 1.0))); shared_ptr o2(new Plane(n, offset)); @@ -623,25 +624,25 @@ void test_normal_and_nearest_points(const BVHModel& o1, #else size_t n = 10000; #endif - FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.}; - std::vector transforms; + CoalScalar extents[] = {-2., -2., -2., 2., 2., 2.}; + std::vector transforms; generateRandomTransforms(extents, transforms, n); - Transform3f tf1 = Transform3f::Identity(); - transforms[0] = Transform3f::Identity(); + Transform3s tf1 = Transform3s::Identity(); + transforms[0] = Transform3s::Identity(); CollisionRequest colreq; - colreq.distance_upper_bound = std::numeric_limits::max(); + colreq.distance_upper_bound = std::numeric_limits::max(); colreq.num_max_contacts = 100; CollisionResult colres; DistanceRequest distreq; DistanceResult distres; for (size_t i = 0; i < n; i++) { - Transform3f tf2 = transforms[i]; + Transform3s tf2 = transforms[i]; colres.clear(); distres.clear(); size_t col = collide(&o1, tf1, &o2, tf2, colreq, colres); - FCL_REAL dist = distance(&o1, tf1, &o2, tf2, distreq, distres); + CoalScalar dist = distance(&o1, tf1, &o2, tf2, distreq, distres); if (col) { BOOST_CHECK(dist <= 0.); @@ -651,13 +652,13 @@ void test_normal_and_nearest_points(const BVHModel& o1, BOOST_CHECK(contact.penetration_depth <= 0); BOOST_CHECK(contact.penetration_depth >= colres.distance_lower_bound); - Vec3f cp1 = contact.nearest_points[0]; - Vec3f cp2 = contact.nearest_points[1]; + Vec3s cp1 = contact.nearest_points[0]; + Vec3s cp2 = contact.nearest_points[1]; BOOST_CHECK_CLOSE(contact.penetration_depth, -(cp2 - cp1).norm(), 1e-6); EIGEN_VECTOR_IS_APPROX( cp1, cp2 - contact.penetration_depth * contact.normal, 1e-6); - Vec3f separation_vector = contact.penetration_depth * contact.normal; + Vec3s separation_vector = contact.penetration_depth * contact.normal; EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, 1e-6); if (dist < 0) { @@ -679,14 +680,14 @@ void test_normal_and_nearest_points(const Halfspace& o1, } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_bvh_halfspace) { - Box* box_ptr = new hpp::fcl::Box(1, 1, 1); - hpp::fcl::CollisionGeometryPtr_t b1(box_ptr); - BVHModel o1 = BVHModel(); - generateBVHModel(o1, *box_ptr, Transform3f()); + Box* box_ptr = new coal::Box(1, 1, 1); + coal::CollisionGeometryPtr_t b1(box_ptr); + BVHModel o1 = BVHModel(); + generateBVHModel(o1, *box_ptr, Transform3s()); o1.buildConvexRepresentation(false); - FCL_REAL offset = 0.1; - Vec3f n = Vec3f::Random(); + CoalScalar offset = 0.1; + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); diff --git a/test/obb.cpp b/test/obb.cpp index 117696bb7..134a8daa2 100644 --- a/test/obb.cpp +++ b/test/obb.cpp @@ -40,34 +40,34 @@ #include -#include +#include "coal/narrowphase/narrowphase.h" #include "../src/BV/OBB.h" -#include +#include "coal/internal/shape_shape_func.h" #include "utility.h" -using namespace hpp::fcl; +using namespace coal; -void randomOBBs(Vec3f& a, Vec3f& b, FCL_REAL extentNorm) { +void randomOBBs(Vec3s& a, Vec3s& b, CoalScalar extentNorm) { // Extent norm is between 0 and extentNorm on each axis - // a = (Vec3f::Ones()+Vec3f::Random()) * extentNorm / (2*sqrt(3)); - // b = (Vec3f::Ones()+Vec3f::Random()) * extentNorm / (2*sqrt(3)); + // a = (Vec3s::Ones()+Vec3s::Random()) * extentNorm / (2*sqrt(3)); + // b = (Vec3s::Ones()+Vec3s::Random()) * extentNorm / (2*sqrt(3)); - a = extentNorm * Vec3f::Random().cwiseAbs().normalized(); - b = extentNorm * Vec3f::Random().cwiseAbs().normalized(); + a = extentNorm * Vec3s::Random().cwiseAbs().normalized(); + b = extentNorm * Vec3s::Random().cwiseAbs().normalized(); } -void randomTransform(Matrix3f& B, Vec3f& T, const Vec3f& a, const Vec3f& b, - const FCL_REAL extentNorm) { +void randomTransform(Matrix3s& B, Vec3s& T, const Vec3s& a, const Vec3s& b, + const CoalScalar extentNorm) { // TODO Should we scale T to a and b norm ? (void)a; (void)b; (void)extentNorm; - FCL_REAL N = a.norm() + b.norm(); + CoalScalar N = a.norm() + b.norm(); // A translation of norm N ensures there is no collision. // Set translation to be between 0 and 2 * N; - T = (Vec3f::Random() / sqrt(3)) * 1.5 * N; + T = (Vec3s::Random() / sqrt(3)) * 1.5 * N; // T.setZero(); Quatf q; @@ -90,7 +90,7 @@ typedef std::chrono::high_resolution_clock clock_type; typedef clock_type::duration duration_type; const char* sep = ",\t"; -const FCL_REAL eps = 1.5e-7; +const CoalScalar eps = 1.5e-7; const Eigen::IOFormat py_fmt(Eigen::FullPrecision, 0, ", ", // Coeff separator @@ -103,22 +103,22 @@ const Eigen::IOFormat py_fmt(Eigen::FullPrecision, 0, namespace obbDisjoint_impls { /// @return true if OBB are disjoint. -bool distance(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, - FCL_REAL& distance) { +bool distance(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, + CoalScalar& distance) { GJKSolver gjk; Box ba(2 * a), bb(2 * b); - Transform3f tfa, tfb(B, T); + Transform3s tfa, tfb(B, T); - Vec3f p1, p2, normal; + Vec3s p1, p2, normal; bool compute_penetration = true; distance = gjk.shapeDistance(ba, tfa, bb, tfb, compute_penetration, p1, p2, normal); return (distance > gjk.getDistancePrecision(compute_penetration)); } -inline FCL_REAL _computeDistanceForCase1(const Vec3f& T, const Vec3f& a, - const Vec3f& b, const Matrix3f& Bf) { - Vec3f AABB_corner; +inline CoalScalar _computeDistanceForCase1(const Vec3s& T, const Vec3s& a, + const Vec3s& b, const Matrix3s& Bf) { + Vec3s AABB_corner; /* This seems to be slower AABB_corner.noalias() = T.cwiseAbs () - a; AABB_corner.noalias() -= PRODUCT(Bf,b); @@ -132,19 +132,19 @@ inline FCL_REAL _computeDistanceForCase1(const Vec3f& T, const Vec3f& a, AABB_corner.noalias() = T.cwiseAbs() - Bf * b - a; #endif // */ - return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm(); + return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm(); } -inline FCL_REAL _computeDistanceForCase2(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, - const Matrix3f& Bf) { +inline CoalScalar _computeDistanceForCase2(const Matrix3s& B, const Vec3s& T, + const Vec3s& a, const Vec3s& b, + const Matrix3s& Bf) { /* - Vec3f AABB_corner(PRODUCT(B.transpose(), T).cwiseAbs() - b); + Vec3s AABB_corner(PRODUCT(B.transpose(), T).cwiseAbs() - b); AABB_corner.noalias() -= PRODUCT(Bf.transpose(), a); - return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm (); + return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm (); /*/ #if MANUAL_PRODUCT - FCL_REAL s, t = 0; + CoalScalar s, t = 0; s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0]; if (s > 0) t += s * s; s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1]; @@ -153,18 +153,18 @@ inline FCL_REAL _computeDistanceForCase2(const Matrix3f& B, const Vec3f& T, if (s > 0) t += s * s; return t; #else - Vec3f AABB_corner((B.transpose() * T).cwiseAbs() - Bf.transpose() * a - b); - return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm(); + Vec3s AABB_corner((B.transpose() * T).cwiseAbs() - Bf.transpose() * a - b); + return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm(); #endif // */ } -int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b, const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { +int separatingAxisId(const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b, const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { int id = 0; - Matrix3f Bf(B.cwiseAbs()); + Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); @@ -179,14 +179,15 @@ int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a, int ja = 1, ka = 2, jb = 1, kb = 2; for (int ia = 0; ia < 3; ++ia) { for (int ib = 0; ib < 3; ++ib) { - const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); + const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); - const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + - b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); + const CoalScalar diff = + fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); // We need to divide by the norm || Aia x Bib || // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 if (diff > 0) { - FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); + CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { @@ -194,7 +195,7 @@ int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a, } } /* // or - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); + CoalScalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); squaredLowerBoundDistance = diff * diff; if (squaredLowerBoundDistance > breakDistance2 * sinus2) { squaredLowerBoundDistance /= sinus2; @@ -215,10 +216,10 @@ int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a, } // ------------------------ 0 -------------------------------------- -bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b, const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { - Matrix3f Bf(B.cwiseAbs()); +bool withRuntimeLoop(const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b, const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { + Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); @@ -231,14 +232,15 @@ bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a, int ja = 1, ka = 2, jb = 1, kb = 2; for (int ia = 0; ia < 3; ++ia) { for (int ib = 0; ib < 3; ++ib) { - const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); + const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); - const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + - b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); + const CoalScalar diff = + fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); // We need to divide by the norm || Aia x Bib || // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 if (diff > 0) { - FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); + CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { @@ -246,7 +248,7 @@ bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a, } } /* // or - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); + CoalScalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); squaredLowerBoundDistance = diff * diff; if (squaredLowerBoundDistance > breakDistance2 * sinus2) { squaredLowerBoundDistance /= sinus2; @@ -266,30 +268,30 @@ bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a, } // ------------------------ 1 -------------------------------------- -bool withManualLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, - const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { - FCL_REAL t, s; - FCL_REAL diff; - - // Matrix3f Bf = abs(B); +bool withManualLoopUnrolling_1(const Matrix3s& B, const Vec3s& T, + const Vec3s& a, const Vec3s& b, + const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { + CoalScalar t, s; + CoalScalar diff; + + // Matrix3s Bf = abs(B); // Bf += reps; - Matrix3f Bf(B.cwiseAbs()); + Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin - Vec3f AABB_corner(T.cwiseAbs() - Bf * b); - Vec3f diff3(AABB_corner - a); - diff3 = diff3.cwiseMax(Vec3f::Zero()); + Vec3s AABB_corner(T.cwiseAbs() - Bf * b); + Vec3s diff3(AABB_corner - a); + diff3 = diff3.cwiseMax(Vec3s::Zero()); - // for (Vec3f::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]); + // for (Vec3s::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]); squaredLowerBoundDistance = diff3.squaredNorm(); if (squaredLowerBoundDistance > breakDistance2) return true; AABB_corner = (B.transpose() * T).cwiseAbs() - Bf.transpose() * a; // diff3 = | B^T T| - b - Bf^T a diff3 = AABB_corner - b; - diff3 = diff3.cwiseMax(Vec3f::Zero()); + diff3 = diff3.cwiseMax(Vec3s::Zero()); squaredLowerBoundDistance = diff3.squaredNorm(); if (squaredLowerBoundDistance > breakDistance2) return true; @@ -299,7 +301,7 @@ bool withManualLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); - FCL_REAL sinus2; + CoalScalar sinus2; diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1)); // We need to divide by the norm || A0 x B0 || @@ -456,11 +458,11 @@ bool withManualLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, } // ------------------------ 2 -------------------------------------- -bool withManualLoopUnrolling_2(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, - const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { - Matrix3f Bf(B.cwiseAbs()); +bool withManualLoopUnrolling_2(const Matrix3s& B, const Vec3s& T, + const Vec3s& a, const Vec3s& b, + const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { + Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); @@ -470,13 +472,13 @@ bool withManualLoopUnrolling_2(const Matrix3f& B, const Vec3f& T, if (squaredLowerBoundDistance > breakDistance2) return true; // A0 x B0 - FCL_REAL t, s; + CoalScalar t, s; s = T[2] * B(1, 0) - T[1] * B(2, 0); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); - FCL_REAL sinus2; - FCL_REAL diff; + CoalScalar sinus2; + CoalScalar diff; diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1)); // We need to divide by the norm || A0 x B0 || @@ -636,18 +638,18 @@ bool withManualLoopUnrolling_2(const Matrix3f& B, const Vec3f& T, template struct loop_case_1 { - static inline bool run(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b, const Matrix3f& Bf, - const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { - const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); - - const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + - b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); + static inline bool run(const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b, const Matrix3s& Bf, + const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { + const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); + + const CoalScalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); // We need to divide by the norm || Aia x Bib || // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 if (diff > 0) { - FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); + CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { @@ -655,7 +657,7 @@ struct loop_case_1 { } } /* // or - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); + CoalScalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); squaredLowerBoundDistance = diff * diff; if (squaredLowerBoundDistance > breakDistance2 * sinus2) { squaredLowerBoundDistance /= sinus2; @@ -667,11 +669,11 @@ struct loop_case_1 { } }; -bool withTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, - const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { - Matrix3f Bf(B.cwiseAbs()); +bool withTemplateLoopUnrolling_1(const Matrix3s& B, const Vec3s& T, + const Vec3s& a, const Vec3s& b, + const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { + Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); @@ -716,18 +718,18 @@ bool withTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, template struct loop_case_2 { - static inline bool run(int ia, int ja, int ka, const Matrix3f& B, - const Vec3f& T, const Vec3f& a, const Vec3f& b, - const Matrix3f& Bf, const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { - const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); - - const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + - b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); + static inline bool run(int ia, int ja, int ka, const Matrix3s& B, + const Vec3s& T, const Vec3s& a, const Vec3s& b, + const Matrix3s& Bf, const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { + const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); + + const CoalScalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); // We need to divide by the norm || Aia x Bib || // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 if (diff > 0) { - FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); + CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { @@ -735,7 +737,7 @@ struct loop_case_2 { } } /* // or - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); + CoalScalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); squaredLowerBoundDistance = diff * diff; if (squaredLowerBoundDistance > breakDistance2 * sinus2) { squaredLowerBoundDistance /= sinus2; @@ -747,11 +749,11 @@ struct loop_case_2 { } }; -bool withPartialTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, - const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { - Matrix3f Bf(B.cwiseAbs()); +bool withPartialTemplateLoopUnrolling_1(const Matrix3s& B, const Vec3s& T, + const Vec3s& a, const Vec3s& b, + const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { + Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); @@ -780,13 +782,13 @@ bool withPartialTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, } // ------------------------ 5 -------------------------------------- -bool originalWithLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b, const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { - FCL_REAL t, s; - FCL_REAL diff; +bool originalWithLowerBound(const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b, const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { + CoalScalar t, s; + CoalScalar diff; - Matrix3f Bf(B.cwiseAbs()); + Matrix3s Bf(B.cwiseAbs()); squaredLowerBoundDistance = 0; // if any of these tests are one-sided, then the polyhedra are disjoint @@ -852,7 +854,7 @@ bool originalWithLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a, s = T[2] * B(1, 0) - T[1] * B(2, 0); t = ((s < 0.0) ? -s : s); - FCL_REAL sinus2; + CoalScalar sinus2; diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1)); // We need to divide by the norm || A0 x B0 || @@ -1001,15 +1003,15 @@ bool originalWithLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a, } // ------------------------ 6 -------------------------------------- -bool originalWithNoLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b, const FCL_REAL&, - FCL_REAL& squaredLowerBoundDistance) { - FCL_REAL t, s; - const FCL_REAL reps = 1e-6; +bool originalWithNoLowerBound(const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b, const CoalScalar&, + CoalScalar& squaredLowerBoundDistance) { + CoalScalar t, s; + const CoalScalar reps = 1e-6; squaredLowerBoundDistance = 0; - Matrix3f Bf(B.array().abs() + reps); + Matrix3s Bf(B.array().abs() + reps); // Bf += reps; // if any of these tests are one-sided, then the polyhedra are disjoint @@ -1137,8 +1139,8 @@ struct BenchmarkResult { /// - 0-10 identifies a separating axes. /// - 11 means no separatins axes could be found. (distance should be <= 0) int ifId; - FCL_REAL distance; - FCL_REAL squaredLowerBoundDistance; + CoalScalar distance; + CoalScalar squaredLowerBoundDistance; duration_type duration[NB_METHODS]; bool failure; @@ -1171,13 +1173,13 @@ std::ostream& operator<<(std::ostream& os, const BenchmarkResult& br) { return br.print(os); } -BenchmarkResult benchmark_obb_case(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, +BenchmarkResult benchmark_obb_case(const Matrix3s& B, const Vec3s& T, + const Vec3s& a, const Vec3s& b, const CollisionRequest& request, std::size_t N) { - const FCL_REAL breakDistance(request.break_distance + - request.security_margin); - const FCL_REAL breakDistance2 = breakDistance * breakDistance; + const CoalScalar breakDistance(request.break_distance + + request.security_margin); + const CoalScalar breakDistance2 = breakDistance * breakDistance; BenchmarkResult result; // First determine which axis provide the answer @@ -1189,7 +1191,7 @@ BenchmarkResult benchmark_obb_case(const Matrix3f& B, const Vec3f& T, // Sanity check result.failure = true; bool overlap = (result.ifId == 11); - FCL_REAL dist_thr = request.break_distance + request.security_margin; + CoalScalar dist_thr = request.break_distance + request.security_margin; if (!overlap && result.distance <= 0) { std::cerr << "Failure: negative distance for disjoint OBBs."; } else if (!overlap && result.squaredLowerBoundDistance < 0) { @@ -1216,7 +1218,7 @@ BenchmarkResult benchmark_obb_case(const Matrix3f& B, const Vec3f& T, } // Compute time - FCL_REAL tmp; + CoalScalar tmp; clock_type::time_point start, end; // ------------------------ 0 -------------------------------------- @@ -1281,9 +1283,9 @@ std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output) { std::size_t nbFailure = 0; // Create two OBBs axis - Vec3f a, b; - Matrix3f B; - Vec3f T; + Vec3s a, b; + Matrix3s B; + Vec3s T; CollisionRequest request; #ifndef NDEBUG // if debug mode @@ -1295,7 +1297,7 @@ std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output) { static const size_t nbTransformPerOBB = 100; static const size_t nbRunForTimeMeas = 1000; #endif - static const FCL_REAL extentNorm = 1.; + static const CoalScalar extentNorm = 1.; if (output != NULL) *output << BenchmarkResult::headers << '\n'; diff --git a/test/octree.cpp b/test/octree.cpp index 566fb6dd5..d7f3b5d35 100644 --- a/test/octree.cpp +++ b/test/octree.cpp @@ -35,29 +35,29 @@ /** \author Florent Lamiraux */ -#define BOOST_TEST_MODULE FCL_OCTREE +#define BOOST_TEST_MODULE COAL_OCTREE #include #include #include -#include -#include -#include -#include -#include -#include -#include +#include "coal/BVH/BVH_model.h" +#include "coal/collision.h" +#include "coal/distance.h" +#include "coal/hfield.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/internal/BV_splitter.h" #include "utility.h" #include "fcl_resources/config.h" namespace utf = boost::unit_test::framework; -using namespace hpp::fcl; +using namespace coal; -void makeMesh(const std::vector& vertices, +void makeMesh(const std::vector& vertices, const std::vector& triangles, BVHModel& model) { - hpp::fcl::SplitMethodType split_method(hpp::fcl::SPLIT_METHOD_MEAN); + coal::SplitMethodType split_method(coal::SPLIT_METHOD_MEAN); model.bv_splitter.reset(new BVSplitter(split_method)); model.bv_splitter.reset(new BVSplitter(split_method)); @@ -66,27 +66,26 @@ void makeMesh(const std::vector& vertices, model.endModel(); } -hpp::fcl::OcTree makeOctree(const BVHModel& mesh, - const FCL_REAL& resolution) { - Vec3f m(mesh.aabb_local.min_); - Vec3f M(mesh.aabb_local.max_); - hpp::fcl::Box box(resolution, resolution, resolution); - CollisionRequest request(hpp::fcl::CONTACT | hpp::fcl::DISTANCE_LOWER_BOUND, - 1); +coal::OcTree makeOctree(const BVHModel& mesh, + const CoalScalar& resolution) { + Vec3s m(mesh.aabb_local.min_); + Vec3s M(mesh.aabb_local.max_); + coal::Box box(resolution, resolution, resolution); + CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1); CollisionResult result; - Transform3f tfBox; + Transform3s tfBox; octomap::OcTreePtr_t octree(new octomap::OcTree(resolution)); - for (FCL_REAL x = resolution * floor(m[0] / resolution); x <= M[0]; + for (CoalScalar x = resolution * floor(m[0] / resolution); x <= M[0]; x += resolution) { - for (FCL_REAL y = resolution * floor(m[1] / resolution); y <= M[1]; + for (CoalScalar y = resolution * floor(m[1] / resolution); y <= M[1]; y += resolution) { - for (FCL_REAL z = resolution * floor(m[2] / resolution); z <= M[2]; + for (CoalScalar z = resolution * floor(m[2] / resolution); z <= M[2]; z += resolution) { - Vec3f center(x + .5 * resolution, y + .5 * resolution, + Vec3s center(x + .5 * resolution, y + .5 * resolution, z + .5 * resolution); tfBox.setTranslation(center); - hpp::fcl::collide(&box, tfBox, &mesh, Transform3f(), request, result); + coal::collide(&box, tfBox, &mesh, Transform3s(), request, result); if (result.isCollision()) { octomap::point3d p((float)center[0], (float)center[1], (float)center[2]); @@ -105,8 +104,8 @@ hpp::fcl::OcTree makeOctree(const BVHModel& mesh, BOOST_AUTO_TEST_CASE(octree_mesh) { Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ", "", "", "(", ")"); - FCL_REAL resolution(10.); - std::vector pRob, pEnv; + CoalScalar resolution(10.); + std::vector pRob, pEnv; std::vector tRob, tEnv; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "rob.obj").string().c_str(), pRob, tRob); @@ -120,7 +119,7 @@ BOOST_AUTO_TEST_CASE(octree_mesh) { envMesh.computeLocalAABB(); // Load octree built from envMesh by makeOctree(envMesh, resolution) OcTree envOctree( - hpp::fcl::loadOctreeFile((path / "env.octree").string(), resolution)); + coal::loadOctreeFile((path / "env.octree").string(), resolution)); std::cout << "Finished loading octree." << std::endl; @@ -137,40 +136,39 @@ BOOST_AUTO_TEST_CASE(octree_mesh) { { const std::vector bytes = envOctree.tobytes(); BOOST_CHECK(bytes.size() > 0 && bytes.size() <= envOctree.toBoxes().size() * - 3 * sizeof(FCL_REAL)); + 3 * sizeof(CoalScalar)); } - std::vector transforms; - FCL_REAL extents[] = {-2000, -2000, 0, 2000, 2000, 2000}; + std::vector transforms; + CoalScalar extents[] = {-2000, -2000, 0, 2000, 2000, 2000}; #ifndef NDEBUG // if debug mode std::size_t N = 100; #else std::size_t N = 10000; #endif - N = hpp::fcl::getNbRun(utf::master_test_suite().argc, - utf::master_test_suite().argv, N); + N = coal::getNbRun(utf::master_test_suite().argc, + utf::master_test_suite().argv, N); generateRandomTransforms(extents, transforms, 2 * N); - CollisionRequest request(hpp::fcl::CONTACT | hpp::fcl::DISTANCE_LOWER_BOUND, - 1); + CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1); for (std::size_t i = 0; i < N; ++i) { CollisionResult resultMesh; CollisionResult resultOctree; - Transform3f tf1(transforms[2 * i]); - Transform3f tf2(transforms[2 * i + 1]); + Transform3s tf1(transforms[2 * i]); + Transform3s tf2(transforms[2 * i + 1]); ; // Test collision between meshes with random transform for robot. - hpp::fcl::collide(&robMesh, tf1, &envMesh, tf2, request, resultMesh); + coal::collide(&robMesh, tf1, &envMesh, tf2, request, resultMesh); // Test collision between mesh and octree for the same transform. - hpp::fcl::collide(&robMesh, tf1, &envOctree, tf2, request, resultOctree); + coal::collide(&robMesh, tf1, &envOctree, tf2, request, resultOctree); bool resMesh(resultMesh.isCollision()); bool resOctree(resultOctree.isCollision()); BOOST_CHECK(!resMesh || resOctree); if (!resMesh && resOctree) { - hpp::fcl::DistanceRequest dreq; - hpp::fcl::DistanceResult dres; - hpp::fcl::distance(&robMesh, tf1, &envMesh, tf2, dreq, dres); + coal::DistanceRequest dreq; + coal::DistanceResult dres; + coal::distance(&robMesh, tf1, &envMesh, tf2, dreq, dres); std::cout << "distance mesh mesh: " << dres.min_distance << std::endl; BOOST_CHECK(dres.min_distance < sqrt(2.) * resolution); } @@ -180,8 +178,8 @@ BOOST_AUTO_TEST_CASE(octree_mesh) { BOOST_AUTO_TEST_CASE(octree_height_field) { Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ", "", "", "(", ")"); - FCL_REAL resolution(10.); - std::vector pEnv; + CoalScalar resolution(10.); + std::vector pEnv; std::vector tEnv; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), pEnv, tEnv); @@ -193,57 +191,56 @@ BOOST_AUTO_TEST_CASE(octree_height_field) { envMesh.computeLocalAABB(); // Load octree built from envMesh by makeOctree(envMesh, resolution) OcTree envOctree( - hpp::fcl::loadOctreeFile((path / "env.octree").string(), resolution)); + coal::loadOctreeFile((path / "env.octree").string(), resolution)); std::cout << "Finished loading octree." << std::endl; // Building hfield - const FCL_REAL x_dim = 10, y_dim = 20; + const CoalScalar x_dim = 10, y_dim = 20; const int nx = 100, ny = 100; - const FCL_REAL max_altitude = 1., min_altitude = 0.; - const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude); + const CoalScalar max_altitude = 1., min_altitude = 0.; + const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude); HeightField hfield(x_dim, y_dim, heights, min_altitude); hfield.computeLocalAABB(); - std::vector transforms; - FCL_REAL extents[] = {-2000, -2000, 0, 2000, 2000, 2000}; + std::vector transforms; + CoalScalar extents[] = {-2000, -2000, 0, 2000, 2000, 2000}; #ifndef NDEBUG // if debug mode std::size_t N = 1000; #else std::size_t N = 100000; #endif - N = hpp::fcl::getNbRun(utf::master_test_suite().argc, - utf::master_test_suite().argv, N); + N = coal::getNbRun(utf::master_test_suite().argc, + utf::master_test_suite().argv, N); generateRandomTransforms(extents, transforms, 2 * N); - CollisionRequest request(hpp::fcl::CONTACT | hpp::fcl::DISTANCE_LOWER_BOUND, - 1); + CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1); for (std::size_t i = 0; i < N; ++i) { CollisionResult resultBox; CollisionResult resultHfield1, resultHfield2; - Transform3f tf1(transforms[2 * i]); - Transform3f tf2(transforms[2 * i + 1]); + Transform3s tf1(transforms[2 * i]); + Transform3s tf2(transforms[2 * i + 1]); Box box; - Transform3f box_tf; + Transform3s box_tf; constructBox(hfield.aabb_local, tf2, box, box_tf); // Test collision between octree and equivalent box. - hpp::fcl::collide(&envOctree, tf1, &box, box_tf, request, resultBox); + coal::collide(&envOctree, tf1, &box, box_tf, request, resultBox); // Test collision between octree and hfield. - hpp::fcl::collide(&envOctree, tf1, &hfield, tf2, request, resultHfield1); - hpp::fcl::collide(&hfield, tf2, &envOctree, tf1, request, resultHfield2); + coal::collide(&envOctree, tf1, &hfield, tf2, request, resultHfield1); + coal::collide(&hfield, tf2, &envOctree, tf1, request, resultHfield2); bool resBox(resultBox.isCollision()); bool resHfield(resultHfield1.isCollision()); BOOST_CHECK(resBox == resHfield); BOOST_CHECK(resultHfield1.isCollision() == resultHfield2.isCollision()); if (!resBox && resHfield) { - hpp::fcl::DistanceRequest dreq; - hpp::fcl::DistanceResult dres; - hpp::fcl::distance(&envMesh, tf1, &box, box_tf, dreq, dres); + coal::DistanceRequest dreq; + coal::DistanceResult dres; + coal::distance(&envMesh, tf1, &box, box_tf, dreq, dres); std::cout << "distance mesh box: " << dres.min_distance << std::endl; BOOST_CHECK(dres.min_distance < sqrt(2.) * resolution); } diff --git a/test/profiling.cpp b/test/profiling.cpp index 6c11dac7d..ab97a9648 100644 --- a/test/profiling.cpp +++ b/test/profiling.cpp @@ -1,33 +1,33 @@ // Copyright (c) 2017, Joseph Mirabel // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // -// This file is part of hpp-fcl. -// hpp-fcl is free software: you can redistribute it +// This file is part of Coal. +// Coal is free software: you can redistribute it // and/or modify it under the terms of the GNU Lesser General Public // License as published by the Free Software Foundation, either version // 3 of the License, or (at your option) any later version. // -// hpp-fcl is distributed in the hope that it will be +// Coal is distributed in the hope that it will be // useful, but WITHOUT ANY WARRANTY; without even the implied warranty // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Lesser Public License for more details. You should have // received a copy of the GNU Lesser General Public License along with -// hpp-fcl. If not, see . +// Coal. If not, see . #include -#include -#include -#include -#include -#include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/collision.h" +#include "coal/BVH/BVH_model.h" +#include "coal/collision_utility.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/collision_func_matrix.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/mesh_loader/assimp.h" #include "utility.h" #include "fcl_resources/config.h" -using namespace hpp::fcl; +using namespace coal; CollisionFunctionMatrix lookupTable; bool supportedPair(const CollisionGeometry* o1, const CollisionGeometry* o2) { @@ -44,7 +44,7 @@ bool supportedPair(const CollisionGeometry* o1, const CollisionGeometry* o2) { template CollisionGeometryPtr_t objToGeom(const std::string& filename) { - std::vector pt; + std::vector pt; std::vector tri; loadOBJFile(filename.c_str(), pt, tri); @@ -60,7 +60,7 @@ CollisionGeometryPtr_t objToGeom(const std::string& filename) { template CollisionGeometryPtr_t meshToGeom(const std::string& filename, - const Vec3f& scale = Vec3f(1, 1, 1)) { + const Vec3s& scale = Vec3s(1, 1, 1)) { shared_ptr > model(new BVHModel()); loadPolyhedronFromResource(filename, scale, model); return model; @@ -82,10 +82,10 @@ struct Results { Results(size_t i) : rs(i), times((Eigen::DenseIndex)i) {} }; -void collide(const std::vector& tf, const CollisionGeometry* o1, +void collide(const std::vector& tf, const CollisionGeometry* o1, const CollisionGeometry* o2, const CollisionRequest& request, Results& results) { - Transform3f Id; + Transform3s Id; BenchTimer timer; for (std::size_t i = 0; i < tf.size(); ++i) { timer.start(); @@ -117,7 +117,7 @@ size_t Ntransform = 1; #else size_t Ntransform = 100; #endif -FCL_REAL limit = 20; +CoalScalar limit = 20; bool verbose = false; #define OUT(x) \ @@ -190,16 +190,16 @@ Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv) { iarg += 3; if (iarg < argc && strcmp(argv[iarg], "crop") == 0) { CHECK_PARAM_NB(6, Crop); - hpp::fcl::AABB aabb(Vec3f(atof(argv[iarg + 1]), atof(argv[iarg + 2]), - atof(argv[iarg + 3])), - Vec3f(atof(argv[iarg + 4]), atof(argv[iarg + 5]), - atof(argv[iarg + 6]))); + coal::AABB aabb(Vec3s(atof(argv[iarg + 1]), atof(argv[iarg + 2]), + atof(argv[iarg + 3])), + Vec3s(atof(argv[iarg + 4]), atof(argv[iarg + 5]), + atof(argv[iarg + 6]))); OUT("Cropping " << aabb.min_.transpose() << " ---- " << aabb.max_.transpose() << " ..."); o->computeLocalAABB(); OUT("Mesh AABB is " << o->aabb_local.min_.transpose() << " ---- " << o->aabb_local.max_.transpose() << " ..."); - o.reset(extract(o.get(), Transform3f(), aabb)); + o.reset(extract(o.get(), Transform3s(), aabb)); if (!o) throw std::invalid_argument("Failed to crop."); OUT("Crop has " << dynamic_pointer_cast >(o)->num_tris << " triangles"); @@ -221,7 +221,7 @@ Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv) { } int main(int argc, char** argv) { - std::vector transforms; + std::vector transforms; CollisionRequest request; @@ -231,14 +231,14 @@ int main(int argc, char** argv) { Geometry first = makeGeomFromParam(iarg, argc, argv); Geometry second = makeGeomFromParam(iarg, argc, argv); - FCL_REAL extents[] = {-limit, -limit, -limit, limit, limit, limit}; + CoalScalar extents[] = {-limit, -limit, -limit, limit, limit, limit}; generateRandomTransforms(extents, transforms, Ntransform); printResultHeaders(); Results results(Ntransform); collide(transforms, first.o.get(), second.o.get(), request, results); printResults(first, second, results); } else { - FCL_REAL extents[] = {-limit, -limit, -limit, limit, limit, limit}; + CoalScalar extents[] = {-limit, -limit, -limit, limit, limit, limit}; generateRandomTransforms(extents, transforms, Ntransform); boost::filesystem::path path(TEST_RESOURCES_DIR); @@ -249,11 +249,11 @@ int main(int argc, char** argv) { geoms.push_back(Geometry("Cone", new Cone(2, 1))); geoms.push_back(Geometry("Cylinder", new Cylinder(2, 1))); // geoms.push_back(Geometry ("Plane" , new Plane - // (Vec3f(1,1,1).normalized(), 0) )); + // (Vec3s(1,1,1).normalized(), 0) )); // geoms.push_back(Geometry ("Halfspace" , new Halfspace - // (Vec3f(1,1,1).normalized(), 0) )); + // (Vec3s(1,1,1).normalized(), 0) )); // not implemented // geoms.push_back(Geometry ("TriangleP" , new TriangleP - // (Vec3f(0,1,0), Vec3f(0,0,1), Vec3f(-1,0,0)) )); + // (Vec3s(0,1,0), Vec3s(0,0,1), Vec3s(-1,0,0)) )); geoms.push_back(Geometry("rob BVHModel", objToGeom((path / "rob.obj").string()))); diff --git a/test/python_unit/CMakeLists.txt b/test/python_unit/CMakeLists.txt index 8f7e098e0..1e3a95298 100644 --- a/test/python_unit/CMakeLists.txt +++ b/test/python_unit/CMakeLists.txt @@ -6,7 +6,7 @@ SET(${PROJECT_NAME}_PYTHON_TESTS pickling ) -ADD_DEPENDENCIES(build_tests hppfcl) +ADD_DEPENDENCIES(build_tests ${PROJECT_NAME}_pywrap) FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) - ADD_PYTHON_UNIT_TEST("py-${TEST}" "test/python_unit/${TEST}.py" "python") -ENDFOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) + ADD_PYTHON_UNIT_TEST("${PROJECT_NAME}-py-${TEST}" "test/python_unit/${TEST}.py" "python") +ENDFOREACH() diff --git a/test/python_unit/api.py b/test/python_unit/api.py index d982dc611..f15b08f60 100644 --- a/test/python_unit/api.py +++ b/test/python_unit/api.py @@ -1,30 +1,30 @@ import unittest from test_case import TestCase -import hppfcl +import coal import numpy as np class TestMainAPI(TestCase): def test_collision(self): - capsule = hppfcl.Capsule(1.0, 2.0) - M1 = hppfcl.Transform3f() - M2 = hppfcl.Transform3f(np.eye(3), np.array([3, 0, 0])) + capsule = coal.Capsule(1.0, 2.0) + M1 = coal.Transform3s() + M2 = coal.Transform3s(np.eye(3), np.array([3, 0, 0])) - req = hppfcl.CollisionRequest() - res = hppfcl.CollisionResult() + req = coal.CollisionRequest() + res = coal.CollisionResult() - self.assertTrue(not hppfcl.collide(capsule, M1, capsule, M2, req, res)) + self.assertTrue(not coal.collide(capsule, M1, capsule, M2, req, res)) def test_distance(self): - capsule = hppfcl.Capsule(1.0, 2.0) - M1 = hppfcl.Transform3f() - M2 = hppfcl.Transform3f(np.eye(3), np.array([3, 0, 0])) + capsule = coal.Capsule(1.0, 2.0) + M1 = coal.Transform3s() + M2 = coal.Transform3s(np.eye(3), np.array([3, 0, 0])) - req = hppfcl.DistanceRequest() - res = hppfcl.DistanceResult() + req = coal.DistanceRequest() + res = coal.DistanceResult() - self.assertTrue(hppfcl.distance(capsule, M1, capsule, M2, req, res) > 0) + self.assertTrue(coal.distance(capsule, M1, capsule, M2, req, res) > 0) if __name__ == "__main__": diff --git a/test/python_unit/collision.py b/test/python_unit/collision.py index d14ece98f..afd69d503 100644 --- a/test/python_unit/collision.py +++ b/test/python_unit/collision.py @@ -1,47 +1,47 @@ import unittest from test_case import TestCase -import hppfcl +import coal import numpy as np def tetahedron(): - pts = hppfcl.StdVec_Vec3f() + pts = coal.StdVec_Vec3s() pts.append(np.array((0, 0, 0))) pts.append(np.array((0, 1, 0))) pts.append(np.array((1, 0, 0))) pts.append(np.array((0, 0, 1))) - tri = hppfcl.StdVec_Triangle() - tri.append(hppfcl.Triangle(0, 1, 2)) - tri.append(hppfcl.Triangle(0, 1, 3)) - tri.append(hppfcl.Triangle(0, 2, 3)) - tri.append(hppfcl.Triangle(1, 2, 3)) - return hppfcl.Convex(pts, tri) + tri = coal.StdVec_Triangle() + tri.append(coal.Triangle(0, 1, 2)) + tri.append(coal.Triangle(0, 1, 3)) + tri.append(coal.Triangle(0, 2, 3)) + tri.append(coal.Triangle(1, 2, 3)) + return coal.Convex(pts, tri) class TestMainAPI(TestCase): def test_convex_halfspace(self): convex = tetahedron() - halfspace = hppfcl.Halfspace(np.array((0, 0, 1)), 0) + halfspace = coal.Halfspace(np.array((0, 0, 1)), 0) - req = hppfcl.CollisionRequest() - res = hppfcl.CollisionResult() + req = coal.CollisionRequest() + res = coal.CollisionResult() - M1 = hppfcl.Transform3f() - M2 = hppfcl.Transform3f(np.eye(3), np.array([0, 0, -0.1])) + M1 = coal.Transform3s() + M2 = coal.Transform3s(np.eye(3), np.array([0, 0, -0.1])) res.clear() - hppfcl.collide(convex, M1, halfspace, M2, req, res) - self.assertFalse(hppfcl.collide(convex, M1, halfspace, M2, req, res)) + coal.collide(convex, M1, halfspace, M2, req, res) + self.assertFalse(coal.collide(convex, M1, halfspace, M2, req, res)) - M1 = hppfcl.Transform3f() - M2 = hppfcl.Transform3f(np.eye(3), np.array([0, 0, 0.1])) + M1 = coal.Transform3s() + M2 = coal.Transform3s(np.eye(3), np.array([0, 0, 0.1])) res.clear() - self.assertTrue(hppfcl.collide(convex, M1, halfspace, M2, req, res)) + self.assertTrue(coal.collide(convex, M1, halfspace, M2, req, res)) - M1 = hppfcl.Transform3f() - M2 = hppfcl.Transform3f(np.eye(3), np.array([0, 0, 2])) + M1 = coal.Transform3s() + M2 = coal.Transform3s(np.eye(3), np.array([0, 0, 2])) res.clear() - self.assertTrue(hppfcl.collide(convex, M1, halfspace, M2, req, res)) + self.assertTrue(coal.collide(convex, M1, halfspace, M2, req, res)) if __name__ == "__main__": diff --git a/test/python_unit/collision_manager.py b/test/python_unit/collision_manager.py index 46f7fa925..241fda1c6 100644 --- a/test/python_unit/collision_manager.py +++ b/test/python_unit/collision_manager.py @@ -1,21 +1,21 @@ -import hppfcl as fcl +import coal import numpy as np -sphere = fcl.Sphere(0.5) -sphere_obj = fcl.CollisionObject(sphere) +sphere = coal.Sphere(0.5) +sphere_obj = coal.CollisionObject(sphere) -M_sphere = fcl.Transform3f.Identity() +M_sphere = coal.Transform3s.Identity() M_sphere.setTranslation(np.array([-0.6, 0.0, 0.0])) sphere_obj.setTransform(M_sphere) -box = fcl.Box(np.array([0.5, 0.5, 0.5])) -box_obj = fcl.CollisionObject(box) +box = coal.Box(np.array([0.5, 0.5, 0.5])) +box_obj = coal.CollisionObject(box) -M_box = fcl.Transform3f.Identity() +M_box = coal.Transform3s.Identity() M_box.setTranslation(np.array([-0.6, 0.0, 0.0])) box_obj.setTransform(M_box) -collision_manager = fcl.DynamicAABBTreeCollisionManager() +collision_manager = coal.DynamicAABBTreeCollisionManager() collision_manager.registerObject(sphere_obj) collision_manager.registerObject(box_obj) @@ -24,7 +24,7 @@ collision_manager.setup() # Perform collision detection -callback = fcl.CollisionCallBackDefault() +callback = coal.CollisionCallBackDefault() collision_manager.collide(sphere_obj, callback) assert callback.data.result.numContacts() == 1 diff --git a/test/python_unit/geometric_shapes.py b/test/python_unit/geometric_shapes.py index 4d6735b10..c7774e316 100644 --- a/test/python_unit/geometric_shapes.py +++ b/test/python_unit/geometric_shapes.py @@ -1,16 +1,16 @@ import unittest from test_case import TestCase -import hppfcl +import coal import numpy as np class TestGeometricShapes(TestCase): def test_capsule(self): - capsule = hppfcl.Capsule(1.0, 2.0) - self.assertIsInstance(capsule, hppfcl.Capsule) - self.assertIsInstance(capsule, hppfcl.ShapeBase) - self.assertIsInstance(capsule, hppfcl.CollisionGeometry) - self.assertEqual(capsule.getNodeType(), hppfcl.NODE_TYPE.GEOM_CAPSULE) + capsule = coal.Capsule(1.0, 2.0) + self.assertIsInstance(capsule, coal.Capsule) + self.assertIsInstance(capsule, coal.ShapeBase) + self.assertIsInstance(capsule, coal.CollisionGeometry) + self.assertEqual(capsule.getNodeType(), coal.NODE_TYPE.GEOM_CAPSULE) self.assertEqual(capsule.radius, 1.0) self.assertEqual(capsule.halfLength, 1.0) capsule.radius = 3.0 @@ -47,11 +47,11 @@ def test_capsule(self): self.assertApprox(Ic, I0_ref) def test_box1(self): - box = hppfcl.Box(np.array([1.0, 2.0, 3.0])) - self.assertIsInstance(box, hppfcl.Box) - self.assertIsInstance(box, hppfcl.ShapeBase) - self.assertIsInstance(box, hppfcl.CollisionGeometry) - self.assertEqual(box.getNodeType(), hppfcl.NODE_TYPE.GEOM_BOX) + box = coal.Box(np.array([1.0, 2.0, 3.0])) + self.assertIsInstance(box, coal.Box) + self.assertIsInstance(box, coal.ShapeBase) + self.assertIsInstance(box, coal.CollisionGeometry) + self.assertEqual(box.getNodeType(), coal.NODE_TYPE.GEOM_BOX) self.assertTrue(np.array_equal(box.halfSide, np.array([0.5, 1.0, 1.5]))) box.halfSide = np.array([4.0, 5.0, 6.0]) self.assertTrue(np.array_equal(box.halfSide, np.array([4.0, 5.0, 6.0]))) @@ -73,21 +73,21 @@ def test_box1(self): self.assertApprox(Ic, I0_ref) def test_box2(self): - box = hppfcl.Box(1.0, 2.0, 3) - self.assertIsInstance(box, hppfcl.Box) - self.assertIsInstance(box, hppfcl.ShapeBase) - self.assertIsInstance(box, hppfcl.CollisionGeometry) - self.assertEqual(box.getNodeType(), hppfcl.NODE_TYPE.GEOM_BOX) + box = coal.Box(1.0, 2.0, 3) + self.assertIsInstance(box, coal.Box) + self.assertIsInstance(box, coal.ShapeBase) + self.assertIsInstance(box, coal.CollisionGeometry) + self.assertEqual(box.getNodeType(), coal.NODE_TYPE.GEOM_BOX) self.assertEqual(box.halfSide[0], 0.5) self.assertEqual(box.halfSide[1], 1.0) self.assertEqual(box.halfSide[2], 1.5) def test_sphere(self): - sphere = hppfcl.Sphere(1.0) - self.assertIsInstance(sphere, hppfcl.Sphere) - self.assertIsInstance(sphere, hppfcl.ShapeBase) - self.assertIsInstance(sphere, hppfcl.CollisionGeometry) - self.assertEqual(sphere.getNodeType(), hppfcl.NODE_TYPE.GEOM_SPHERE) + sphere = coal.Sphere(1.0) + self.assertIsInstance(sphere, coal.Sphere) + self.assertIsInstance(sphere, coal.ShapeBase) + self.assertIsInstance(sphere, coal.CollisionGeometry) + self.assertEqual(sphere.getNodeType(), coal.NODE_TYPE.GEOM_SPHERE) self.assertEqual(sphere.radius, 1.0) sphere.radius = 2.0 self.assertEqual(sphere.radius, 2.0) @@ -103,11 +103,11 @@ def test_sphere(self): self.assertApprox(Ic, I0_ref) def test_cylinder(self): - cylinder = hppfcl.Cylinder(1.0, 2.0) - self.assertIsInstance(cylinder, hppfcl.Cylinder) - self.assertIsInstance(cylinder, hppfcl.ShapeBase) - self.assertIsInstance(cylinder, hppfcl.CollisionGeometry) - self.assertEqual(cylinder.getNodeType(), hppfcl.NODE_TYPE.GEOM_CYLINDER) + cylinder = coal.Cylinder(1.0, 2.0) + self.assertIsInstance(cylinder, coal.Cylinder) + self.assertIsInstance(cylinder, coal.ShapeBase) + self.assertIsInstance(cylinder, coal.CollisionGeometry) + self.assertEqual(cylinder.getNodeType(), coal.NODE_TYPE.GEOM_CYLINDER) self.assertEqual(cylinder.radius, 1.0) self.assertEqual(cylinder.halfLength, 1.0) cylinder.radius = 3.0 @@ -128,11 +128,11 @@ def test_cylinder(self): self.assertApprox(Ic, I0_ref) def test_cone(self): - cone = hppfcl.Cone(1.0, 2.0) - self.assertIsInstance(cone, hppfcl.Cone) - self.assertIsInstance(cone, hppfcl.ShapeBase) - self.assertIsInstance(cone, hppfcl.CollisionGeometry) - self.assertEqual(cone.getNodeType(), hppfcl.NODE_TYPE.GEOM_CONE) + cone = coal.Cone(1.0, 2.0) + self.assertIsInstance(cone, coal.Cone) + self.assertIsInstance(cone, coal.ShapeBase) + self.assertIsInstance(cone, coal.CollisionGeometry) + self.assertEqual(cone.getNodeType(), coal.NODE_TYPE.GEOM_CONE) self.assertEqual(cone.radius, 1.0) self.assertEqual(cone.halfLength, 1.0) cone.radius = 3.0 @@ -155,13 +155,13 @@ def test_cone(self): self.assertApprox(Ic, Ic_ref) def test_BVH(self): - bvh = hppfcl.BVHModelOBBRSS() + bvh = coal.BVHModelOBBRSS() self.assertEqual(bvh.num_vertices, 0) self.assertEqual(bvh.vertices().shape, (0, 3)) def test_convex(self): - verts = hppfcl.StdVec_Vec3f() - faces = hppfcl.StdVec_Triangle() + verts = coal.StdVec_Vec3s() + faces = coal.StdVec_Triangle() verts.extend( [ np.array([0, 0, 0]), @@ -169,12 +169,12 @@ def test_convex(self): np.array([1, 0, 0]), ] ) - faces.append(hppfcl.Triangle(0, 1, 2)) - hppfcl.Convex(verts, faces) + faces.append(coal.Triangle(0, 1, 2)) + coal.Convex(verts, faces) verts.append(np.array([0, 0, 1])) try: - hppfcl.Convex.convexHull(verts, False, None) + coal.Convex.convexHull(verts, False, None) qhullAvailable = True except Exception as e: self.assertIn( @@ -183,11 +183,11 @@ def test_convex(self): qhullAvailable = False if qhullAvailable: - hppfcl.Convex.convexHull(verts, False, "") - hppfcl.Convex.convexHull(verts, True, "") + coal.Convex.convexHull(verts, False, "") + coal.Convex.convexHull(verts, True, "") try: - hppfcl.Convex.convexHull(verts[:3], False, None) + coal.Convex.convexHull(verts[:3], False, None) except Exception as e: self.assertIn( "You shouldn't use this function with less than 4 points.", str(e) diff --git a/test/python_unit/pickling.py b/test/python_unit/pickling.py index 1e20fe393..c6143d879 100644 --- a/test/python_unit/pickling.py +++ b/test/python_unit/pickling.py @@ -1,23 +1,23 @@ import unittest from test_case import TestCase -import hppfcl +import coal import pickle import numpy as np def tetahedron(): - pts = hppfcl.StdVec_Vec3f() + pts = coal.StdVec_Vec3s() pts.append(np.array((0, 0, 0))) pts.append(np.array((0, 1, 0))) pts.append(np.array((1, 0, 0))) pts.append(np.array((0, 0, 1))) - tri = hppfcl.StdVec_Triangle() - tri.append(hppfcl.Triangle(0, 1, 2)) - tri.append(hppfcl.Triangle(0, 1, 3)) - tri.append(hppfcl.Triangle(0, 2, 3)) - tri.append(hppfcl.Triangle(1, 2, 3)) - return hppfcl.Convex(pts, tri) + tri = coal.StdVec_Triangle() + tri.append(coal.Triangle(0, 1, 2)) + tri.append(coal.Triangle(0, 1, 3)) + tri.append(coal.Triangle(0, 2, 3)) + tri.append(coal.Triangle(1, 2, 3)) + return coal.Convex(pts, tri) class TestGeometryPickling(TestCase): @@ -30,28 +30,28 @@ def pickling(self, obj): self.assertTrue(obj == obj2) def test_all_shapes(self): - box = hppfcl.Box(1.0, 2.0, 3.0) + box = coal.Box(1.0, 2.0, 3.0) self.pickling(box) - sphere = hppfcl.Sphere(1.0) + sphere = coal.Sphere(1.0) self.pickling(sphere) - ellipsoid = hppfcl.Ellipsoid(1.0, 2.0, 3.0) + ellipsoid = coal.Ellipsoid(1.0, 2.0, 3.0) self.pickling(ellipsoid) convex = tetahedron() self.pickling(convex) - capsule = hppfcl.Capsule(1.0, 2.0) + capsule = coal.Capsule(1.0, 2.0) self.pickling(capsule) - cylinder = hppfcl.Cylinder(1.0, 2.0) + cylinder = coal.Cylinder(1.0, 2.0) self.pickling(cylinder) - plane = hppfcl.Plane(np.array([0.0, 0.0, 1.0]), 2.0) + plane = coal.Plane(np.array([0.0, 0.0, 1.0]), 2.0) self.pickling(plane) - half_space = hppfcl.Halfspace(np.array([0.0, 0.0, 1.0]), 2.0) + half_space = coal.Halfspace(np.array([0.0, 0.0, 1.0]), 2.0) self.pickling(half_space) diff --git a/test/security_margin.cpp b/test/security_margin.cpp index aee545577..5993343da 100644 --- a/test/security_margin.cpp +++ b/test/security_margin.cpp @@ -32,53 +32,53 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#define BOOST_TEST_MODULE FCL_SECURITY_MARGIN +#define BOOST_TEST_MODULE COAL_SECURITY_MARGIN #include #include #include -#include -#include -#include -#include -#include -#include -#include +#include "coal/distance.h" +#include "coal/math/transform.h" +#include "coal/collision.h" +#include "coal/collision_object.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/shape/geometric_shape_to_BVH_model.h" #include "utility.h" -using namespace hpp::fcl; -using hpp::fcl::CollisionGeometryPtr_t; -using hpp::fcl::CollisionObject; -using hpp::fcl::CollisionRequest; -using hpp::fcl::CollisionResult; -using hpp::fcl::DistanceRequest; -using hpp::fcl::DistanceResult; -using hpp::fcl::Transform3f; -using hpp::fcl::Vec3f; +using namespace coal; +using coal::CollisionGeometryPtr_t; +using coal::CollisionObject; +using coal::CollisionRequest; +using coal::CollisionResult; +using coal::DistanceRequest; +using coal::DistanceResult; +using coal::Transform3s; +using coal::Vec3s; #define MATH_SQUARED(x) (x * x) BOOST_AUTO_TEST_CASE(aabb_aabb) { - CollisionGeometryPtr_t b1(new hpp::fcl::Box(1, 1, 1)); - CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1)); + CollisionGeometryPtr_t b1(new coal::Box(1, 1, 1)); + CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1)); - const Transform3f tf1; - const Transform3f tf2_collision(Vec3f(0, 1, 1)); - hpp::fcl::Box s1(1, 1, 1); - hpp::fcl::Box s2(1, 1, 1); + const Transform3s tf1; + const Transform3s tf2_collision(Vec3s(0, 1, 1)); + coal::Box s1(1, 1, 1); + coal::Box s2(1, 1, 1); const double tol = 1e-8; AABB bv1, bv2; - computeBV(s1, Transform3f(), bv1); - computeBV(s2, Transform3f(), bv2); + computeBV(s1, Transform3s(), bv1); + computeBV(s2, Transform3s(), bv2); // No security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); AABB bv2_transformed; computeBV(s2, tf2_collision, bv2_transformed); - FCL_REAL sqrDistLowerBound; + CoalScalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(res); @@ -89,11 +89,11 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { { CollisionRequest collisionRequest(CONTACT, 1); const double distance = 0.01; - Transform3f tf2_no_collision( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + Transform3s tf2_no_collision( + Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); AABB bv2_transformed; computeBV(s2, tf2_no_collision, bv2_transformed); - FCL_REAL sqrDistLowerBound; + CoalScalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(!res); @@ -105,11 +105,11 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { CollisionRequest collisionRequest(CONTACT, 1); const double distance = 0.01; collisionRequest.security_margin = distance; - Transform3f tf2_no_collision( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + Transform3s tf2_no_collision( + Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); AABB bv2_transformed; computeBV(s2, tf2_no_collision, bv2_transformed); - FCL_REAL sqrDistLowerBound; + CoalScalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(res); @@ -121,11 +121,11 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { CollisionRequest collisionRequest(CONTACT, 1); const double distance = -0.01; collisionRequest.security_margin = distance; - const Transform3f tf2( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, distance))); + const Transform3s tf2( + Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, distance))); AABB bv2_transformed; computeBV(s2, tf2, bv2_transformed); - FCL_REAL sqrDistLowerBound; + CoalScalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(res); @@ -139,7 +139,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { collisionRequest.security_margin = distance; AABB bv2_transformed; computeBV(s2, tf2_collision, bv2_transformed); - FCL_REAL sqrDistLowerBound; + CoalScalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(!res); @@ -150,17 +150,17 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { } BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) { - CollisionGeometryPtr_t b1(new hpp::fcl::Box(1, 1, 1)); - CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1)); + CollisionGeometryPtr_t b1(new coal::Box(1, 1, 1)); + CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1)); - const Transform3f tf1; - const Transform3f tf2_collision(Vec3f(0, 0, 0)); - hpp::fcl::Box s1(1, 1, 1); - hpp::fcl::Box s2(1, 1, 1); + const Transform3s tf1; + const Transform3s tf2_collision(Vec3s(0, 0, 0)); + coal::Box s1(1, 1, 1); + coal::Box s2(1, 1, 1); AABB bv1, bv2; - computeBV(s1, Transform3f(), bv1); - computeBV(s2, Transform3f(), bv2); + computeBV(s1, Transform3s(), bv1); + computeBV(s2, Transform3s(), bv2); // The two AABB are collocated { @@ -169,7 +169,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) { collisionRequest.security_margin = distance; AABB bv2_transformed; computeBV(s2, tf2_collision, bv2_transformed); - FCL_REAL sqrDistLowerBound; + CoalScalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(!res); @@ -180,11 +180,11 @@ BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) { } BOOST_AUTO_TEST_CASE(sphere_sphere) { - CollisionGeometryPtr_t s1(new hpp::fcl::Sphere(1)); - CollisionGeometryPtr_t s2(new hpp::fcl::Sphere(2)); + CollisionGeometryPtr_t s1(new coal::Sphere(1)); + CollisionGeometryPtr_t s2(new coal::Sphere(2)); - const Transform3f tf1; - const Transform3f tf2_collision(Vec3f(0, 0, 3)); + const Transform3s tf1; + const Transform3s tf2_collision(Vec3s(0, 0, 3)); // NOTE: when comparing a result to zero, **do not use BOOST_CHECK_CLOSE**! // Zero is not close to any other number, so the test will always fail. @@ -206,8 +206,8 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; const double distance = 0.01; - Transform3f tf2_no_collision( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + Transform3s tf2_no_collision( + Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); collide(s1.get(), tf1, s2.get(), tf2_no_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); @@ -220,8 +220,8 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { CollisionResult collisionResult; const double distance = 0.01; collisionRequest.security_margin = distance; - Transform3f tf2( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + Transform3s tf2( + Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); collide(s1.get(), tf1, s2.get(), tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8); @@ -235,8 +235,8 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { CollisionResult collisionResult; const double distance = -0.01; collisionRequest.security_margin = distance; - Transform3f tf2( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + Transform3s tf2( + Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); collide(s1.get(), tf1, s2.get(), tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8); @@ -258,11 +258,11 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { } BOOST_AUTO_TEST_CASE(capsule_capsule) { - CollisionGeometryPtr_t c1(new hpp::fcl::Capsule(0.5, 1.)); - CollisionGeometryPtr_t c2(new hpp::fcl::Capsule(0.5, 1.)); + CollisionGeometryPtr_t c1(new coal::Capsule(0.5, 1.)); + CollisionGeometryPtr_t c2(new coal::Capsule(0.5, 1.)); - const Transform3f tf1; - const Transform3f tf2_collision(Vec3f(0, 1., 0)); + const Transform3s tf1; + const Transform3s tf2_collision(Vec3s(0, 1., 0)); // No security margin - collision { @@ -280,8 +280,8 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; const double distance = 0.01; - Transform3f tf2_no_collision( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0))); + Transform3s tf2_no_collision( + Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0))); collide(c1.get(), tf1, c2.get(), tf2_no_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); @@ -294,8 +294,8 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) { CollisionResult collisionResult; const double distance = 0.01; collisionRequest.security_margin = distance; - Transform3f tf2_no_collision( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0))); + Transform3s tf2_no_collision( + Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0))); collide(c1.get(), tf1, c2.get(), tf2_no_collision, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); @@ -310,8 +310,8 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) { CollisionResult collisionResult; const double distance = -0.01; collisionRequest.security_margin = distance; - Transform3f tf2( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0))); + Transform3s tf2( + Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0))); collide(c1.get(), tf1, c2.get(), tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8); @@ -332,11 +332,11 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) { } BOOST_AUTO_TEST_CASE(box_box) { - CollisionGeometryPtr_t b1(new hpp::fcl::Box(1, 1, 1)); - CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1)); + CollisionGeometryPtr_t b1(new coal::Box(1, 1, 1)); + CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1)); - const Transform3f tf1; - const Transform3f tf2_collision(Vec3f(0, 1, 1)); + const Transform3s tf1; + const Transform3s tf2_collision(Vec3s(0, 1, 1)); const double tol = 1e-3; @@ -355,8 +355,8 @@ BOOST_AUTO_TEST_CASE(box_box) { { CollisionRequest collisionRequest(CONTACT, 1); const double distance = 0.01; - const Transform3f tf2_no_collision( - (tf2_collision.getTranslation() + Vec3f(0, 0, distance)).eval()); + const Transform3s tf2_no_collision( + (tf2_collision.getTranslation() + Vec3s(0, 0, distance)).eval()); CollisionResult collisionResult; collide(b1.get(), tf1, b2.get(), tf2_no_collision, collisionRequest, @@ -394,12 +394,12 @@ BOOST_AUTO_TEST_CASE(box_box) { // Negative security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); - const FCL_REAL distance = -0.01; + const CoalScalar distance = -0.01; collisionRequest.security_margin = distance; CollisionResult collisionResult; - const Transform3f tf2((tf2_collision.getTranslation() + - Vec3f(0, collisionRequest.security_margin, + const Transform3s tf2((tf2_collision.getTranslation() + + Vec3s(0, collisionRequest.security_margin, collisionRequest.security_margin)) .eval()); collide(b1.get(), tf1, b2.get(), tf2, collisionRequest, collisionResult); @@ -411,9 +411,9 @@ BOOST_AUTO_TEST_CASE(box_box) { } template -void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, +void test_shape_shape(const ShapeType1& shape1, const Transform3s& tf1, const ShapeType2& shape2, - const Transform3f& tf2_collision, const FCL_REAL tol) { + const Transform3s& tf2_collision, const CoalScalar tol) { // No security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); @@ -429,8 +429,8 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, { CollisionRequest collisionRequest(CONTACT, 1); const double distance = 0.01; - const Transform3f tf2_no_collision( - (tf2_collision.getTranslation() + Vec3f(0, 0, distance)).eval()); + const Transform3s tf2_no_collision( + (tf2_collision.getTranslation() + Vec3s(0, 0, distance)).eval()); CollisionResult collisionResult; collide(&shape1, tf1, &shape2, tf2_no_collision, collisionRequest, @@ -471,12 +471,12 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, // Negative security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); - const FCL_REAL distance = -0.01; + const CoalScalar distance = -0.01; collisionRequest.security_margin = distance; CollisionResult collisionResult; - const Transform3f tf2((tf2_collision.getTranslation() + - Vec3f(0, collisionRequest.security_margin, + const Transform3s tf2((tf2_collision.getTranslation() + + Vec3s(0, collisionRequest.security_margin, collisionRequest.security_margin)) .eval()); collide(&shape1, tf1, &shape2, tf2, collisionRequest, collisionResult); @@ -488,16 +488,16 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, } BOOST_AUTO_TEST_CASE(sphere_box) { - Box* box_ptr = new hpp::fcl::Box(1, 1, 1); + Box* box_ptr = new coal::Box(1, 1, 1); CollisionGeometryPtr_t b1(box_ptr); BVHModel box_bvh_model = BVHModel(); - generateBVHModel(box_bvh_model, *box_ptr, Transform3f()); + generateBVHModel(box_bvh_model, *box_ptr, Transform3s()); box_bvh_model.buildConvexRepresentation(false); ConvexBase& box_convex = *box_bvh_model.convex.get(); - CollisionGeometryPtr_t s2(new hpp::fcl::Sphere(0.5)); + CollisionGeometryPtr_t s2(new coal::Sphere(0.5)); - const Transform3f tf1; - const Transform3f tf2_collision(Vec3f(0, 0, 1)); + const Transform3s tf1; + const Transform3s tf2_collision(Vec3s(0, 0, 1)); const double tol = 1e-6; diff --git a/test/serialization.cpp b/test/serialization.cpp index 10fb645a8..11609d300 100644 --- a/test/serialization.cpp +++ b/test/serialization.cpp @@ -32,34 +32,35 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#define BOOST_TEST_MODULE FCL_SERIALIZATION +#define BOOST_TEST_MODULE COAL_SERIALIZATION #include #include -#include - -HPP_FCL_COMPILER_DIAGNOSTIC_PUSH -HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#ifdef HPP_FCL_HAS_OCTOMAP -#include +#include "coal/fwd.hh" + +COAL_COMPILER_DIAGNOSTIC_PUSH +COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + +#include "coal/collision.h" + +#include "coal/contact_patch.h" +#include "coal/distance.h" +#include "coal/BV/OBBRSS.h" +#include "coal/BVH/BVH_model.h" + +#include "coal/serialization/collision_data.h" +#include "coal/serialization/contact_patch.h" +#include "coal/serialization/AABB.h" +#include "coal/serialization/BVH_model.h" +#include "coal/serialization/hfield.h" +#include "coal/serialization/transform.h" +#include "coal/serialization/geometric_shapes.h" +#include "coal/serialization/convex.h" +#include "coal/serialization/archive.h" +#include "coal/serialization/memory.h" + +#ifdef COAL_HAS_OCTOMAP +#include "coal/serialization/octree.h" #endif #include "utility.h" @@ -70,7 +71,7 @@ HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS namespace utf = boost::unit_test::framework; -using namespace hpp::fcl; +using namespace coal; template bool check(const T& value, const T& other) { @@ -158,31 +159,31 @@ void test_serialization(const T& value, T& other_value, // -- TXT { const std::string filename = txt_filename.string(); - hpp::fcl::serialization::saveToText(value, filename); + coal::serialization::saveToText(value, filename); BOOST_CHECK(check(value, value)); - hpp::fcl::serialization::loadFromText(other_value, filename); + coal::serialization::loadFromText(other_value, filename); BOOST_CHECK(check(value, other_value)); } // -- String stream (TXT format) { std::stringstream ss_out; - hpp::fcl::serialization::saveToStringStream(value, ss_out); + coal::serialization::saveToStringStream(value, ss_out); BOOST_CHECK(check(value, value)); std::istringstream ss_in(ss_out.str()); - hpp::fcl::serialization::loadFromStringStream(other_value, ss_in); + coal::serialization::loadFromStringStream(other_value, ss_in); BOOST_CHECK(check(value, other_value)); } // -- String { - const std::string str_out = hpp::fcl::serialization::saveToString(value); + const std::string str_out = coal::serialization::saveToString(value); BOOST_CHECK(check(value, value)); const std::string str_in(str_out); - hpp::fcl::serialization::loadFromString(other_value, str_in); + coal::serialization::loadFromString(other_value, str_in); BOOST_CHECK(check(value, other_value)); } } @@ -192,10 +193,10 @@ void test_serialization(const T& value, T& other_value, { const std::string filename = xml_filename.string(); const std::string xml_tag = "value"; - hpp::fcl::serialization::saveToXML(value, filename, xml_tag); + coal::serialization::saveToXML(value, filename, xml_tag); BOOST_CHECK(check(value, value)); - hpp::fcl::serialization::loadFromXML(other_value, filename, xml_tag); + coal::serialization::loadFromXML(other_value, filename, xml_tag); BOOST_CHECK(check(value, other_value)); } } @@ -204,10 +205,10 @@ void test_serialization(const T& value, T& other_value, if (mode & 0x4) { { const std::string filename = bin_filename.string(); - hpp::fcl::serialization::saveToBinary(value, filename); + coal::serialization::saveToBinary(value, filename); BOOST_CHECK(check(value, value)); - hpp::fcl::serialization::loadFromBinary(other_value, filename); + coal::serialization::loadFromBinary(other_value, filename); BOOST_CHECK(check(value, other_value)); } } @@ -216,10 +217,10 @@ void test_serialization(const T& value, T& other_value, if (mode & 0x8) { { boost::asio::streambuf buffer; - hpp::fcl::serialization::saveToBuffer(value, buffer); + coal::serialization::saveToBuffer(value, buffer); BOOST_CHECK(check(value, value)); - hpp::fcl::serialization::loadFromBuffer(other_value, buffer); + coal::serialization::loadFromBuffer(other_value, buffer); BOOST_CHECK(check(value, other_value)); } } @@ -230,11 +231,11 @@ void test_serialization(const T& value, T& other_value, std::shared_ptr ptr = std::make_shared(value); const std::string filename = txt_ptr_filename.string(); - hpp::fcl::serialization::saveToText(ptr, filename); + coal::serialization::saveToText(ptr, filename); BOOST_CHECK(check_ptr(ptr.get(), ptr.get())); std::shared_ptr other_ptr = nullptr; - hpp::fcl::serialization::loadFromText(other_ptr, filename); + coal::serialization::loadFromText(other_ptr, filename); BOOST_CHECK(check_ptr(ptr.get(), other_ptr.get())); } @@ -249,12 +250,12 @@ void test_serialization(const T& value, } BOOST_AUTO_TEST_CASE(test_aabb) { - AABB aabb(-Vec3f::Ones(), Vec3f::Ones()); + AABB aabb(-Vec3s::Ones(), Vec3s::Ones()); test_serialization(aabb); } BOOST_AUTO_TEST_CASE(test_collision_data) { - Contact contact(NULL, NULL, 1, 2, Vec3f::Ones(), Vec3f::Zero(), -10.); + Contact contact(NULL, NULL, 1, 2, Vec3s::Ones(), Vec3s::Zero(), -10.); test_serialization(contact); CollisionRequest collision_request(CONTACT, 10); @@ -281,27 +282,27 @@ BOOST_AUTO_TEST_CASE(test_collision_data) { { // Serializing contact patches. const Halfspace hspace(0, 0, 1, 0); - const FCL_REAL radius = 0.25; - const FCL_REAL height = 1.; + const CoalScalar radius = 0.25; + const CoalScalar height = 1.; const Cylinder cylinder(radius, height); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to have a collision - const FCL_REAL offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, height / 2 - offset)); + const CoalScalar offset = 0.001; + tf2.setTranslation(Vec3s(0, 0, height / 2 - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); if (col_res.isCollision()) { ContactPatchRequest patch_req; ContactPatchResult patch_res(patch_req); - hpp::fcl::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, + patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); // Serialize patch request, result and the patch itself @@ -325,7 +326,7 @@ void checkEqualStdVector(const std::vector& v1, const std::vector& v2) { } BOOST_AUTO_TEST_CASE(test_BVHModel) { - std::vector p1, p2; + std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); @@ -364,9 +365,9 @@ BOOST_AUTO_TEST_CASE(test_BVHModel) { } } -#ifdef HPP_FCL_HAS_QHULL +#ifdef COAL_HAS_QHULL BOOST_AUTO_TEST_CASE(test_Convex) { - std::vector p1; + std::vector p1; std::vector t1; boost::filesystem::path path(TEST_RESOURCES_DIR); @@ -403,12 +404,12 @@ BOOST_AUTO_TEST_CASE(test_Convex) { BOOST_CHECK(ptr.get()); const std::string filename = xml_filename.string(); const std::string tag_name = "CollisionGeometry"; - hpp::fcl::serialization::saveToXML(ptr, filename, tag_name); + coal::serialization::saveToXML(ptr, filename, tag_name); BOOST_CHECK(check(*reinterpret_cast*>(ptr.get()), convex)); std::shared_ptr other_ptr = nullptr; BOOST_CHECK(!other_ptr.get()); - hpp::fcl::serialization::loadFromXML(other_ptr, filename, tag_name); + coal::serialization::loadFromXML(other_ptr, filename, tag_name); BOOST_CHECK( check(convex, *reinterpret_cast*>(other_ptr.get()))); } @@ -416,10 +417,10 @@ BOOST_AUTO_TEST_CASE(test_Convex) { #endif BOOST_AUTO_TEST_CASE(test_HeightField) { - const FCL_REAL min_altitude = -1.; - const FCL_REAL x_dim = 1., y_dim = 2.; + const CoalScalar min_altitude = -1.; + const CoalScalar x_dim = 1., y_dim = 2.; const Eigen::DenseIndex nx = 100, ny = 200; - const MatrixXf heights = MatrixXf::Random(ny, nx); + const MatrixXs heights = MatrixXs::Random(ny, nx); HeightField hfield(x_dim, y_dim, heights, min_altitude); @@ -435,25 +436,25 @@ BOOST_AUTO_TEST_CASE(test_HeightField) { } BOOST_AUTO_TEST_CASE(test_transform) { - Transform3f T; + Transform3s T; T.setQuatRotation(Quaternion3f::UnitRandom()); - T.setTranslation(Vec3f::Random()); + T.setTranslation(Vec3s::Random()); - Transform3f T_copy; + Transform3s T_copy; test_serialization(T, T_copy); } BOOST_AUTO_TEST_CASE(test_shapes) { { - TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(), Vec3f::UnitZ()); + TriangleP triangle(Vec3s::UnitX(), Vec3s::UnitY(), Vec3s::UnitZ()); triangle.setSweptSphereRadius(1.); triangle.computeLocalAABB(); - TriangleP triangle_copy(Vec3f::Random(), Vec3f::Random(), Vec3f::Random()); + TriangleP triangle_copy(Vec3s::Random(), Vec3s::Random(), Vec3s::Random()); test_serialization(triangle, triangle_copy); } { - Box box(Vec3f::UnitX()), box_copy(Vec3f::Random()); + Box box(Vec3s::UnitX()), box_copy(Vec3s::Random()); box.setSweptSphereRadius(1.); box.computeLocalAABB(); test_serialization(box, box_copy); @@ -495,14 +496,14 @@ BOOST_AUTO_TEST_CASE(test_shapes) { } { - Halfspace hs(Vec3f::Random(), 1.), hs_copy(Vec3f::Zero(), 0.); + Halfspace hs(Vec3s::Random(), 1.), hs_copy(Vec3s::Zero(), 0.); hs.setSweptSphereRadius(1.); hs.computeLocalAABB(); test_serialization(hs, hs_copy); } { - Plane plane(Vec3f::Random(), 1.), plane_copy(Vec3f::Zero(), 0.); + Plane plane(Vec3s::Random(), 1.), plane_copy(Vec3s::Zero(), 0.); plane.setSweptSphereRadius(1.); plane.computeLocalAABB(); test_serialization(plane, plane_copy); @@ -511,11 +512,11 @@ BOOST_AUTO_TEST_CASE(test_shapes) { #ifdef HPP_FCL_HAS_QHULL { const size_t num_points = 500; - std::shared_ptr> points = - std::make_shared>(); + std::shared_ptr> points = + std::make_shared>(); points->reserve(num_points); for (size_t i = 0; i < num_points; i++) { - points->emplace_back(Vec3f::Random()); + points->emplace_back(Vec3s::Random()); } using Convex = Convex; std::unique_ptr convex = @@ -530,10 +531,10 @@ BOOST_AUTO_TEST_CASE(test_shapes) { #endif } -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP BOOST_AUTO_TEST_CASE(test_octree) { - const FCL_REAL resolution = 1e-2; - const Matrixx3f points = Matrixx3f::Random(1000, 3); + const CoalScalar resolution = 1e-2; + const MatrixX3s points = MatrixX3s::Random(1000, 3); OcTreePtr_t octree_ptr = makeOctree(points, resolution); const OcTree& octree = *octree_ptr.get(); @@ -573,7 +574,7 @@ BOOST_AUTO_TEST_CASE(test_memory_footprint) { Sphere sphere(1.); BOOST_CHECK(sizeof(Sphere) == computeMemoryFootprint(sphere)); - std::vector p1; + std::vector p1; std::vector t1; boost::filesystem::path path(TEST_RESOURCES_DIR); @@ -592,4 +593,4 @@ BOOST_AUTO_TEST_CASE(test_memory_footprint) { computeMemoryFootprint(m1)); } -HPP_FCL_COMPILER_DIAGNOSTIC_POP +COAL_COMPILER_DIAGNOSTIC_POP diff --git a/test/shape_inflation.cpp b/test/shape_inflation.cpp index e305906bd..cfd321024 100644 --- a/test/shape_inflation.cpp +++ b/test/shape_inflation.cpp @@ -32,88 +32,89 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#define BOOST_TEST_MODULE FCL_SECURITY_MARGIN +#define BOOST_TEST_MODULE COAL_SECURITY_MARGIN #include #include #include -#include -#include -#include -#include -#include -#include +#include "coal/distance.h" +#include "coal/math/transform.h" +#include "coal/collision.h" +#include "coal/collision_object.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/shape/geometric_shapes_utility.h" #include "utility.h" -using namespace hpp::fcl; -using hpp::fcl::CollisionGeometryPtr_t; -using hpp::fcl::CollisionObject; -using hpp::fcl::CollisionRequest; -using hpp::fcl::CollisionResult; -using hpp::fcl::DistanceRequest; -using hpp::fcl::DistanceResult; -using hpp::fcl::Transform3f; -using hpp::fcl::Vec3f; +using namespace coal; +using coal::CollisionGeometryPtr_t; +using coal::CollisionObject; +using coal::CollisionRequest; +using coal::CollisionResult; +using coal::DistanceRequest; +using coal::DistanceResult; +using coal::Transform3s; +using coal::Vec3s; #define MATH_SQUARED(x) (x * x) template -bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol); +bool isApprox(const Shape &s1, const Shape &s2, const CoalScalar tol); -bool isApprox(const FCL_REAL &v1, const FCL_REAL &v2, const FCL_REAL tol) { - typedef Eigen::Matrix ScalarMatrix; - ScalarMatrix m1; +bool isApprox(const CoalScalar &v1, const CoalScalar &v2, + const CoalScalar tol) { + typedef Eigen::Matrix Matrix; + Matrix m1; m1 << v1; - ScalarMatrix m2; + Matrix m2; m2 << v2; return m1.isApprox(m2, tol); } -bool isApprox(const Box &s1, const Box &s2, const FCL_REAL tol) { +bool isApprox(const Box &s1, const Box &s2, const CoalScalar tol) { return s1.halfSide.isApprox(s2.halfSide, tol); } -bool isApprox(const Sphere &s1, const Sphere &s2, const FCL_REAL tol) { +bool isApprox(const Sphere &s1, const Sphere &s2, const CoalScalar tol) { return isApprox(s1.radius, s2.radius, tol); } -bool isApprox(const Ellipsoid &s1, const Ellipsoid &s2, const FCL_REAL tol) { +bool isApprox(const Ellipsoid &s1, const Ellipsoid &s2, const CoalScalar tol) { return s1.radii.isApprox(s2.radii, tol); } -bool isApprox(const Capsule &s1, const Capsule &s2, const FCL_REAL tol) { +bool isApprox(const Capsule &s1, const Capsule &s2, const CoalScalar tol) { return isApprox(s1.radius, s2.radius, tol) && isApprox(s1.halfLength, s2.halfLength, tol); } -bool isApprox(const Cylinder &s1, const Cylinder &s2, const FCL_REAL tol) { +bool isApprox(const Cylinder &s1, const Cylinder &s2, const CoalScalar tol) { return isApprox(s1.radius, s2.radius, tol) && isApprox(s1.halfLength, s2.halfLength, tol); } -bool isApprox(const Cone &s1, const Cone &s2, const FCL_REAL tol) { +bool isApprox(const Cone &s1, const Cone &s2, const CoalScalar tol) { return isApprox(s1.radius, s2.radius, tol) && isApprox(s1.halfLength, s2.halfLength, tol); } -bool isApprox(const TriangleP &s1, const TriangleP &s2, const FCL_REAL tol) { +bool isApprox(const TriangleP &s1, const TriangleP &s2, const CoalScalar tol) { return s1.a.isApprox(s2.a, tol) && s1.b.isApprox(s2.b, tol) && s1.c.isApprox(s2.c, tol); } -bool isApprox(const Halfspace &s1, const Halfspace &s2, const FCL_REAL tol) { +bool isApprox(const Halfspace &s1, const Halfspace &s2, const CoalScalar tol) { return isApprox(s1.d, s2.d, tol) && s1.n.isApprox(s2.n, tol); } template -void test(const Shape &original_shape, const FCL_REAL inflation, - const FCL_REAL tol = 1e-8) { +void test(const Shape &original_shape, const CoalScalar inflation, + const CoalScalar tol = 1e-8) { // Zero inflation { - const FCL_REAL inflation = 0.; + const CoalScalar inflation = 0.; const auto &inflation_result = original_shape.inflated(inflation); - const Transform3f &shift = inflation_result.second; + const Transform3s &shift = inflation_result.second; const Shape &inflated_shape = inflation_result.first; BOOST_CHECK(isApprox(original_shape, inflated_shape, tol)); @@ -124,13 +125,13 @@ void test(const Shape &original_shape, const FCL_REAL inflation, { const auto &inflation_result = original_shape.inflated(inflation); const Shape &inflated_shape = inflation_result.first; - const Transform3f &inflation_shift = inflation_result.second; + const Transform3s &inflation_shift = inflation_result.second; BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol)); const auto &deflation_result = inflated_shape.inflated(-inflation); const Shape &deflated_shape = deflation_result.first; - const Transform3f &deflation_shift = deflation_result.second; + const Transform3s &deflation_shift = deflation_result.second; BOOST_CHECK(isApprox(original_shape, deflated_shape, tol)); BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol)); @@ -140,13 +141,13 @@ void test(const Shape &original_shape, const FCL_REAL inflation, { const auto &inflation_result = original_shape.inflated(-inflation); const Shape &inflated_shape = inflation_result.first; - const Transform3f &inflation_shift = inflation_result.second; + const Transform3s &inflation_shift = inflation_result.second; BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol)); const auto &deflation_result = inflated_shape.inflated(+inflation); const Shape &deflated_shape = deflation_result.first; - const Transform3f &deflation_shift = deflation_result.second; + const Transform3s &deflation_shift = deflation_result.second; BOOST_CHECK(isApprox(original_shape, deflated_shape, tol)); BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol)); @@ -154,45 +155,45 @@ void test(const Shape &original_shape, const FCL_REAL inflation, } template -void test_throw(const Shape &shape, const FCL_REAL inflation) { +void test_throw(const Shape &shape, const CoalScalar inflation) { BOOST_REQUIRE_THROW(shape.inflated(inflation), std::invalid_argument); } template -void test_no_throw(const Shape &shape, const FCL_REAL inflation) { +void test_no_throw(const Shape &shape, const CoalScalar inflation) { BOOST_REQUIRE_NO_THROW(shape.inflated(inflation)); } BOOST_AUTO_TEST_CASE(test_inflate) { - const hpp::fcl::Sphere sphere(1); + const coal::Sphere sphere(1); test(sphere, 0.01, 1e-8); test_throw(sphere, -1.1); test_no_throw(sphere, 1.); - const hpp::fcl::Box box(1, 1, 1); + const coal::Box box(1, 1, 1); test(box, 0.01, 1e-8); test_throw(box, -0.6); - const hpp::fcl::Ellipsoid ellipsoid(1, 2, 3); + const coal::Ellipsoid ellipsoid(1, 2, 3); test(ellipsoid, 0.01, 1e-8); test_throw(ellipsoid, -1.1); - const hpp::fcl::Capsule capsule(1., 2.); + const coal::Capsule capsule(1., 2.); test(capsule, 0.01, 1e-8); test_throw(capsule, -1.1); - const hpp::fcl::Cylinder cylinder(1., 2.); + const coal::Cylinder cylinder(1., 2.); test(cylinder, 0.01, 1e-8); test_throw(cylinder, -1.1); - const hpp::fcl::Cone cone(1., 4.); + const coal::Cone cone(1., 4.); test(cone, 0.01, 1e-8); test_throw(cone, -1.1); - const hpp::fcl::Halfspace halfspace(Vec3f::UnitZ(), 0.); + const coal::Halfspace halfspace(Vec3s::UnitZ(), 0.); test(halfspace, 0.01, 1e-8); - // const hpp::fcl::TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(), - // Vec3f::UnitZ()); + // const coal::TriangleP triangle(Vec3s::UnitX(), Vec3s::UnitY(), + // Vec3s::UnitZ()); // test(triangle, 0.01, 1e-8); } diff --git a/test/shape_mesh_consistency.cpp b/test/shape_mesh_consistency.cpp index 0c0e7c907..6b8d35e33 100644 --- a/test/shape_mesh_consistency.cpp +++ b/test/shape_mesh_consistency.cpp @@ -35,20 +35,20 @@ /** \author Jia Pan */ -#define BOOST_TEST_MODULE FCL_SHAPE_MESH_CONSISTENCY +#define BOOST_TEST_MODULE COAL_SHAPE_MESH_CONSISTENCY #include -#include -#include -#include -#include +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shape_to_BVH_model.h" +#include "coal/distance.h" +#include "coal/collision.h" #include "utility.h" -using namespace hpp::fcl; +using namespace coal; #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p)) -FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10}; +CoalScalar extents[6] = {0, 0, 0, 10, 10, 10}; BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) { Sphere s1(20); @@ -56,39 +56,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) { BVHModel s1_rss; BVHModel s2_rss; - generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; - Transform3f pose; + Transform3s pose; - pose.setTranslation(Vec3f(50, 0, 0)); + pose.setTranslation(Vec3s(50, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -108,30 +108,30 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) { 0.05); } - pose.setTranslation(Vec3f(40.1, 0, 0)); + pose.setTranslation(Vec3s(40.1, 0, 0)); res.clear(), res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -159,39 +159,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox) { BVHModel s1_rss; BVHModel s2_rss; - generateBVHModel(s1_rss, s1, Transform3f()); - generateBVHModel(s2_rss, s2, Transform3f()); + generateBVHModel(s1_rss, s1, Transform3s()); + generateBVHModel(s2_rss, s2, Transform3s()); DistanceRequest request; DistanceResult res, res1; - Transform3f pose; + Transform3s pose; - pose.setTranslation(Vec3f(50, 0, 0)); + pose.setTranslation(Vec3s(50, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -211,31 +211,31 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox) { 0.01); } - pose.setTranslation(Vec3f(15.1, 0, 0)); + pose.setTranslation(Vec3s(15.1, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -263,39 +263,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder) { BVHModel s1_rss; BVHModel s2_rss; - generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; - Transform3f pose; + Transform3s pose; - pose.setTranslation(Vec3f(20, 0, 0)); + pose.setTranslation(Vec3s(20, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -316,31 +316,31 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder) { } pose.setTranslation( - Vec3f(15, 0, 0)); // libccd cannot use small value here :( + Vec3s(15, 0, 0)); // libccd cannot use small value here :( res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -368,38 +368,38 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone) { BVHModel s1_rss; BVHModel s2_rss; - generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; - Transform3f pose; + Transform3s pose; - pose.setTranslation(Vec3f(20, 0, 0)); + pose.setTranslation(Vec3s(20, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -420,31 +420,31 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone) { } pose.setTranslation( - Vec3f(15, 0, 0)); // libccd cannot use small value here :( + Vec3s(15, 0, 0)); // libccd cannot use small value here :( res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -471,39 +471,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK) { BVHModel s1_rss; BVHModel s2_rss; - generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; - Transform3f pose; + Transform3s pose; - pose.setTranslation(Vec3f(50, 0, 0)); + pose.setTranslation(Vec3s(50, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -523,31 +523,31 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK) { 0.05); } - pose.setTranslation(Vec3f(40.1, 0, 0)); + pose.setTranslation(Vec3s(40.1, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -575,39 +575,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK) { BVHModel s1_rss; BVHModel s2_rss; - generateBVHModel(s1_rss, s1, Transform3f()); - generateBVHModel(s2_rss, s2, Transform3f()); + generateBVHModel(s1_rss, s1, Transform3s()); + generateBVHModel(s2_rss, s2, Transform3s()); DistanceRequest request; DistanceResult res, res1; - Transform3f pose; + Transform3s pose; - pose.setTranslation(Vec3f(50, 0, 0)); + pose.setTranslation(Vec3s(50, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -627,31 +627,31 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK) { 0.01); } - pose.setTranslation(Vec3f(15.1, 0, 0)); + pose.setTranslation(Vec3s(15.1, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -679,39 +679,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK) { BVHModel s1_rss; BVHModel s2_rss; - generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; - Transform3f pose; + Transform3s pose; - pose.setTranslation(Vec3f(20, 0, 0)); + pose.setTranslation(Vec3s(20, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -743,31 +743,31 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK) { fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } - pose.setTranslation(Vec3f(10.1, 0, 0)); + pose.setTranslation(Vec3s(10.1, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -795,39 +795,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK) { BVHModel s1_rss; BVHModel s2_rss; - generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; - Transform3f pose; + Transform3s pose; - pose.setTranslation(Vec3f(20, 0, 0)); + pose.setTranslation(Vec3s(20, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -847,31 +847,31 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK) { 0.05); } - pose.setTranslation(Vec3f(10.1, 0, 0)); + pose.setTranslation(Vec3s(10.1, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -900,17 +900,17 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); + generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); CollisionResult result; bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision @@ -918,195 +918,195 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) { // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(40, 0, 0)); - pose_aabb.setTranslation(Vec3f(40, 0, 0)); - pose_obb.setTranslation(Vec3f(40, 0, 0)); + pose.setTranslation(Vec3s(40, 0, 0)); + pose_aabb.setTranslation(Vec3s(40, 0, 0)); + pose_obb.setTranslation(Vec3s(40, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(30, 0, 0)); - pose_aabb.setTranslation(Vec3f(30, 0, 0)); - pose_obb.setTranslation(Vec3f(30, 0, 0)); + pose.setTranslation(Vec3s(30, 0, 0)); + pose_aabb.setTranslation(Vec3s(30, 0, 0)); + pose_obb.setTranslation(Vec3s(30, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(29.9, 0, 0)); + pose.setTranslation(Vec3s(29.9, 0, 0)); pose_aabb.setTranslation( - Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation( - Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(-29.9, 0, 0)); + pose.setTranslation(Vec3s(-29.9, 0, 0)); pose_aabb.setTranslation( - Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation( - Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(-30, 0, 0)); - pose_aabb.setTranslation(Vec3f(-30, 0, 0)); - pose_obb.setTranslation(Vec3f(-30, 0, 0)); + pose.setTranslation(Vec3s(-30, 0, 0)); + pose_aabb.setTranslation(Vec3s(-30, 0, 0)); + pose_obb.setTranslation(Vec3s(-30, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } @@ -1119,17 +1119,17 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f()); - generateBVHModel(s2_aabb, s2, Transform3f()); - generateBVHModel(s1_obb, s1, Transform3f()); - generateBVHModel(s2_obb, s2, Transform3f()); + generateBVHModel(s1_aabb, s1, Transform3s()); + generateBVHModel(s2_aabb, s2, Transform3s()); + generateBVHModel(s1_obb, s1, Transform3s()); + generateBVHModel(s2_obb, s2, Transform3s()); CollisionRequest request(false, 1, false); CollisionResult result; bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision @@ -1137,95 +1137,95 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) { // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(15.01, 0, 0)); - pose_aabb.setTranslation(Vec3f(15.01, 0, 0)); - pose_obb.setTranslation(Vec3f(15.01, 0, 0)); + pose.setTranslation(Vec3s(15.01, 0, 0)); + pose_aabb.setTranslation(Vec3s(15.01, 0, 0)); + pose_obb.setTranslation(Vec3s(15.01, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(14.99, 0, 0)); - pose_aabb.setTranslation(Vec3f(14.99, 0, 0)); - pose_obb.setTranslation(Vec3f(14.99, 0, 0)); + pose.setTranslation(Vec3s(14.99, 0, 0)); + pose_aabb.setTranslation(Vec3s(14.99, 0, 0)); + pose_obb.setTranslation(Vec3s(14.99, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); } @@ -1238,17 +1238,17 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3f()); - generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3f()); + generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3s()); + generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3s()); CollisionRequest request(false, 1, false); CollisionResult result; bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision @@ -1256,95 +1256,95 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) { // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(22.4, 0, 0)); - pose_aabb.setTranslation(Vec3f(22.4, 0, 0)); - pose_obb.setTranslation(Vec3f(22.4, 0, 0)); + pose.setTranslation(Vec3s(22.4, 0, 0)); + pose_aabb.setTranslation(Vec3s(22.4, 0, 0)); + pose_obb.setTranslation(Vec3s(22.4, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(22.51, 0, 0)); - pose_aabb.setTranslation(Vec3f(22.51, 0, 0)); - pose_obb.setTranslation(Vec3f(22.51, 0, 0)); + pose.setTranslation(Vec3s(22.51, 0, 0)); + pose_aabb.setTranslation(Vec3s(22.51, 0, 0)); + pose_obb.setTranslation(Vec3s(22.51, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } @@ -1357,80 +1357,80 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); + generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); CollisionResult result; bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; - pose.setTranslation(Vec3f(9.99, 0, 0)); - pose_aabb.setTranslation(Vec3f(9.99, 0, 0)); - pose_obb.setTranslation(Vec3f(9.99, 0, 0)); + pose.setTranslation(Vec3s(9.99, 0, 0)); + pose_aabb.setTranslation(Vec3s(9.99, 0, 0)); + pose_obb.setTranslation(Vec3s(9.99, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(10.01, 0, 0)); - pose_aabb.setTranslation(Vec3f(10.01, 0, 0)); - pose_obb.setTranslation(Vec3f(10.01, 0, 0)); + pose.setTranslation(Vec3s(10.01, 0, 0)); + pose_aabb.setTranslation(Vec3s(10.01, 0, 0)); + pose_obb.setTranslation(Vec3s(10.01, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } @@ -1443,144 +1443,144 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); + generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); CollisionResult result; bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; - pose.setTranslation(Vec3f(9.9, 0, 0)); - pose_aabb.setTranslation(Vec3f(9.9, 0, 0)); - pose_obb.setTranslation(Vec3f(9.9, 0, 0)); + pose.setTranslation(Vec3s(9.9, 0, 0)); + pose_aabb.setTranslation(Vec3s(9.9, 0, 0)); + pose_obb.setTranslation(Vec3s(9.9, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(10.1, 0, 0)); - pose_aabb.setTranslation(Vec3f(10.1, 0, 0)); - pose_obb.setTranslation(Vec3f(10.1, 0, 0)); + pose.setTranslation(Vec3s(10.1, 0, 0)); + pose_aabb.setTranslation(Vec3s(10.1, 0, 0)); + pose_obb.setTranslation(Vec3s(10.1, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(0, 0, 9.9)); - pose_aabb.setTranslation(Vec3f(0, 0, 9.9)); - pose_obb.setTranslation(Vec3f(0, 0, 9.9)); + pose.setTranslation(Vec3s(0, 0, 9.9)); + pose_aabb.setTranslation(Vec3s(0, 0, 9.9)); + pose_obb.setTranslation(Vec3s(0, 0, 9.9)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(0, 0, 10.1)); - pose_aabb.setTranslation(Vec3f(0, 0, 10.1)); - pose_obb.setTranslation(Vec3f(0, 0, 10.1)); + pose.setTranslation(Vec3s(0, 0, 10.1)); + pose_aabb.setTranslation(Vec3s(0, 0, 10.1)); + pose_obb.setTranslation(Vec3s(0, 0, 10.1)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } @@ -1592,10 +1592,10 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); + generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); @@ -1603,7 +1603,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision @@ -1611,195 +1611,195 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(40, 0, 0)); - pose_aabb.setTranslation(Vec3f(40, 0, 0)); - pose_obb.setTranslation(Vec3f(40, 0, 0)); + pose.setTranslation(Vec3s(40, 0, 0)); + pose_aabb.setTranslation(Vec3s(40, 0, 0)); + pose_obb.setTranslation(Vec3s(40, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(30, 0, 0)); - pose_aabb.setTranslation(Vec3f(30, 0, 0)); - pose_obb.setTranslation(Vec3f(30, 0, 0)); + pose.setTranslation(Vec3s(30, 0, 0)); + pose_aabb.setTranslation(Vec3s(30, 0, 0)); + pose_obb.setTranslation(Vec3s(30, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(29.9, 0, 0)); + pose.setTranslation(Vec3s(29.9, 0, 0)); pose_aabb.setTranslation( - Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation( - Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(-29.9, 0, 0)); + pose.setTranslation(Vec3s(-29.9, 0, 0)); pose_aabb.setTranslation( - Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation( - Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(-30, 0, 0)); - pose_aabb.setTranslation(Vec3f(-30, 0, 0)); - pose_obb.setTranslation(Vec3f(-30, 0, 0)); + pose.setTranslation(Vec3s(-30, 0, 0)); + pose_aabb.setTranslation(Vec3s(-30, 0, 0)); + pose_obb.setTranslation(Vec3s(-30, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } @@ -1812,10 +1812,10 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f()); - generateBVHModel(s2_aabb, s2, Transform3f()); - generateBVHModel(s1_obb, s1, Transform3f()); - generateBVHModel(s2_obb, s2, Transform3f()); + generateBVHModel(s1_aabb, s1, Transform3s()); + generateBVHModel(s2_aabb, s2, Transform3s()); + generateBVHModel(s1_obb, s1, Transform3s()); + generateBVHModel(s2_obb, s2, Transform3s()); CollisionRequest request(false, 1, false); @@ -1823,7 +1823,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) { bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision @@ -1831,95 +1831,95 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) { // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(15.01, 0, 0)); - pose_aabb.setTranslation(Vec3f(15.01, 0, 0)); - pose_obb.setTranslation(Vec3f(15.01, 0, 0)); + pose.setTranslation(Vec3s(15.01, 0, 0)); + pose_aabb.setTranslation(Vec3s(15.01, 0, 0)); + pose_obb.setTranslation(Vec3s(15.01, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(14.99, 0, 0)); - pose_aabb.setTranslation(Vec3f(14.99, 0, 0)); - pose_obb.setTranslation(Vec3f(14.99, 0, 0)); + pose.setTranslation(Vec3s(14.99, 0, 0)); + pose_aabb.setTranslation(Vec3s(14.99, 0, 0)); + pose_obb.setTranslation(Vec3s(14.99, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); } @@ -1932,10 +1932,10 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3f()); - generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3f()); + generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3s()); + generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3s()); CollisionRequest request(false, 1, false); @@ -1943,7 +1943,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) { bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision @@ -1951,95 +1951,95 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) { // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(22.4, 0, 0)); - pose_aabb.setTranslation(Vec3f(22.4, 0, 0)); - pose_obb.setTranslation(Vec3f(22.4, 0, 0)); + pose.setTranslation(Vec3s(22.4, 0, 0)); + pose_aabb.setTranslation(Vec3s(22.4, 0, 0)); + pose_obb.setTranslation(Vec3s(22.4, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(22.51, 0, 0)); - pose_aabb.setTranslation(Vec3f(22.51, 0, 0)); - pose_obb.setTranslation(Vec3f(22.51, 0, 0)); + pose.setTranslation(Vec3s(22.51, 0, 0)); + pose_aabb.setTranslation(Vec3s(22.51, 0, 0)); + pose_obb.setTranslation(Vec3s(22.51, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } @@ -2052,10 +2052,10 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); + generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); @@ -2063,70 +2063,70 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) { bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; - pose.setTranslation(Vec3f(9.99, 0, 0)); - pose_aabb.setTranslation(Vec3f(9.99, 0, 0)); - pose_obb.setTranslation(Vec3f(9.99, 0, 0)); + pose.setTranslation(Vec3s(9.99, 0, 0)); + pose_aabb.setTranslation(Vec3s(9.99, 0, 0)); + pose_obb.setTranslation(Vec3s(9.99, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(10.01, 0, 0)); - pose_aabb.setTranslation(Vec3f(10.01, 0, 0)); - pose_obb.setTranslation(Vec3f(10.01, 0, 0)); + pose.setTranslation(Vec3s(10.01, 0, 0)); + pose_aabb.setTranslation(Vec3s(10.01, 0, 0)); + pose_obb.setTranslation(Vec3s(10.01, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } @@ -2139,10 +2139,10 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); + generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); @@ -2150,133 +2150,133 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) { bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; - pose.setTranslation(Vec3f(9.9, 0, 0)); - pose_aabb.setTranslation(Vec3f(9.9, 0, 0)); - pose_obb.setTranslation(Vec3f(9.9, 0, 0)); + pose.setTranslation(Vec3s(9.9, 0, 0)); + pose_aabb.setTranslation(Vec3s(9.9, 0, 0)); + pose_obb.setTranslation(Vec3s(9.9, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(10.1, 0, 0)); - pose_aabb.setTranslation(Vec3f(10.1, 0, 0)); - pose_obb.setTranslation(Vec3f(10.1, 0, 0)); + pose.setTranslation(Vec3s(10.1, 0, 0)); + pose_aabb.setTranslation(Vec3s(10.1, 0, 0)); + pose_obb.setTranslation(Vec3s(10.1, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(0, 0, 9.9)); - pose_aabb.setTranslation(Vec3f(0, 0, 9.9)); - pose_obb.setTranslation(Vec3f(0, 0, 9.9)); + pose.setTranslation(Vec3s(0, 0, 9.9)); + pose_aabb.setTranslation(Vec3s(0, 0, 9.9)); + pose_obb.setTranslation(Vec3s(0, 0, 9.9)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(0, 0, 10.1)); - pose_aabb.setTranslation(Vec3f(0, 0, 10.1)); - pose_obb.setTranslation(Vec3f(0, 0, 10.1)); + pose.setTranslation(Vec3s(0, 0, 10.1)); + pose_aabb.setTranslation(Vec3s(0, 0, 10.1)); + pose_obb.setTranslation(Vec3s(0, 0, 10.1)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } diff --git a/test/simple.cpp b/test/simple.cpp index 99a7ee521..fd1f7a399 100644 --- a/test/simple.cpp +++ b/test/simple.cpp @@ -1,244 +1,246 @@ -#define BOOST_TEST_MODULE FCL_SIMPLE -#include - -#include -#include -#include -#include "fcl_resources/config.h" -#include - -using namespace hpp::fcl; - -static FCL_REAL epsilon = 1e-6; - -static bool approx(FCL_REAL x, FCL_REAL y) { return std::abs(x - y) < epsilon; } - -BOOST_AUTO_TEST_CASE(projection_test_line) { - Vec3f v1(0, 0, 0); - Vec3f v2(2, 0, 0); - - Vec3f p(1, 0, 0); - Project::ProjectResult res = Project::projectLine(v1, v2, p); - BOOST_CHECK(res.encode == 3); - BOOST_CHECK(approx(res.sqr_distance, 0)); - BOOST_CHECK(approx(res.parameterization[0], 0.5)); - BOOST_CHECK(approx(res.parameterization[1], 0.5)); - - p = Vec3f(-1, 0, 0); - res = Project::projectLine(v1, v2, p); - BOOST_CHECK(res.encode == 1); - BOOST_CHECK(approx(res.sqr_distance, 1)); - BOOST_CHECK(approx(res.parameterization[0], 1)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - - p = Vec3f(3, 0, 0); - res = Project::projectLine(v1, v2, p); - BOOST_CHECK(res.encode == 2); - BOOST_CHECK(approx(res.sqr_distance, 1)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 1)); -} - -BOOST_AUTO_TEST_CASE(projection_test_triangle) { - Vec3f v1(0, 0, 1); - Vec3f v2(0, 1, 0); - Vec3f v3(1, 0, 0); - - Vec3f p(1, 1, 1); - Project::ProjectResult res = Project::projectTriangle(v1, v2, v3, p); - BOOST_CHECK(res.encode == 7); - BOOST_CHECK(approx(res.sqr_distance, 4 / 3.0)); - BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); - - p = Vec3f(0, 0, 1.5); - res = Project::projectTriangle(v1, v2, v3, p); - BOOST_CHECK(res.encode == 1); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 1)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - - p = Vec3f(1.5, 0, 0); - res = Project::projectTriangle(v1, v2, v3, p); - BOOST_CHECK(res.encode == 4); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 1)); - - p = Vec3f(0, 1.5, 0); - res = Project::projectTriangle(v1, v2, v3, p); - BOOST_CHECK(res.encode == 2); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 1)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - - p = Vec3f(1, 1, 0); - res = Project::projectTriangle(v1, v2, v3, p); - BOOST_CHECK(res.encode == 6); - BOOST_CHECK(approx(res.sqr_distance, 0.5)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 0.5)); - BOOST_CHECK(approx(res.parameterization[2], 0.5)); - - p = Vec3f(1, 0, 1); - res = Project::projectTriangle(v1, v2, v3, p); - BOOST_CHECK(res.encode == 5); - BOOST_CHECK(approx(res.sqr_distance, 0.5)); - BOOST_CHECK(approx(res.parameterization[0], 0.5)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 0.5)); - - p = Vec3f(0, 1, 1); - res = Project::projectTriangle(v1, v2, v3, p); - BOOST_CHECK(res.encode == 3); - BOOST_CHECK(approx(res.sqr_distance, 0.5)); - BOOST_CHECK(approx(res.parameterization[0], 0.5)); - BOOST_CHECK(approx(res.parameterization[1], 0.5)); - BOOST_CHECK(approx(res.parameterization[2], 0)); -} - -BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { - Vec3f v1(0, 0, 1); - Vec3f v2(0, 1, 0); - Vec3f v3(1, 0, 0); - Vec3f v4(1, 1, 1); - - Vec3f p(0.5, 0.5, 0.5); - Project::ProjectResult res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 15); - BOOST_CHECK(approx(res.sqr_distance, 0)); - BOOST_CHECK(approx(res.parameterization[0], 0.25)); - BOOST_CHECK(approx(res.parameterization[1], 0.25)); - BOOST_CHECK(approx(res.parameterization[2], 0.25)); - BOOST_CHECK(approx(res.parameterization[3], 0.25)); - - p = Vec3f(0, 0, 0); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 7); - BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[3], 0)); - - p = Vec3f(0, 1, 1); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 11); - BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); - - p = Vec3f(1, 1, 0); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 14); - BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); - - p = Vec3f(1, 0, 1); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 13); - BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); - - p = Vec3f(1.5, 1.5, 1.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 8); - BOOST_CHECK(approx(res.sqr_distance, 0.75)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - BOOST_CHECK(approx(res.parameterization[3], 1)); - - p = Vec3f(1.5, -0.5, -0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 4); - BOOST_CHECK(approx(res.sqr_distance, 0.75)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 1)); - BOOST_CHECK(approx(res.parameterization[3], 0)); - - p = Vec3f(-0.5, -0.5, 1.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 1); - BOOST_CHECK(approx(res.sqr_distance, 0.75)); - BOOST_CHECK(approx(res.parameterization[0], 1)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - BOOST_CHECK(approx(res.parameterization[3], 0)); - - p = Vec3f(-0.5, 1.5, -0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 2); - BOOST_CHECK(approx(res.sqr_distance, 0.75)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 1)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - BOOST_CHECK(approx(res.parameterization[3], 0)); - - p = Vec3f(0.5, -0.5, 0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 5); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 0.5)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 0.5)); - BOOST_CHECK(approx(res.parameterization[3], 0)); - - p = Vec3f(0.5, 1.5, 0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 10); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 0.5)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - BOOST_CHECK(approx(res.parameterization[3], 0.5)); - - p = Vec3f(1.5, 0.5, 0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 12); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 0.5)); - BOOST_CHECK(approx(res.parameterization[3], 0.5)); - - p = Vec3f(-0.5, 0.5, 0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 3); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 0.5)); - BOOST_CHECK(approx(res.parameterization[1], 0.5)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - BOOST_CHECK(approx(res.parameterization[3], 0)); - - p = Vec3f(0.5, 0.5, 1.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 9); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 0.5)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - BOOST_CHECK(approx(res.parameterization[3], 0.5)); - - p = Vec3f(0.5, 0.5, -0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 6); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 0.5)); - BOOST_CHECK(approx(res.parameterization[2], 0.5)); - BOOST_CHECK(approx(res.parameterization[3], 0)); -} +#define BOOST_TEST_MODULE COAL_SIMPLE +#include + +#include "coal/internal/intersect.h" +#include "coal/collision.h" +#include "coal/BVH/BVH_model.h" +#include "fcl_resources/config.h" +#include + +using namespace coal; + +static CoalScalar epsilon = 1e-6; + +static bool approx(CoalScalar x, CoalScalar y) { + return std::abs(x - y) < epsilon; +} + +BOOST_AUTO_TEST_CASE(projection_test_line) { + Vec3s v1(0, 0, 0); + Vec3s v2(2, 0, 0); + + Vec3s p(1, 0, 0); + Project::ProjectResult res = Project::projectLine(v1, v2, p); + BOOST_CHECK(res.encode == 3); + BOOST_CHECK(approx(res.sqr_distance, 0)); + BOOST_CHECK(approx(res.parameterization[0], 0.5)); + BOOST_CHECK(approx(res.parameterization[1], 0.5)); + + p = Vec3s(-1, 0, 0); + res = Project::projectLine(v1, v2, p); + BOOST_CHECK(res.encode == 1); + BOOST_CHECK(approx(res.sqr_distance, 1)); + BOOST_CHECK(approx(res.parameterization[0], 1)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + + p = Vec3s(3, 0, 0); + res = Project::projectLine(v1, v2, p); + BOOST_CHECK(res.encode == 2); + BOOST_CHECK(approx(res.sqr_distance, 1)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 1)); +} + +BOOST_AUTO_TEST_CASE(projection_test_triangle) { + Vec3s v1(0, 0, 1); + Vec3s v2(0, 1, 0); + Vec3s v3(1, 0, 0); + + Vec3s p(1, 1, 1); + Project::ProjectResult res = Project::projectTriangle(v1, v2, v3, p); + BOOST_CHECK(res.encode == 7); + BOOST_CHECK(approx(res.sqr_distance, 4 / 3.0)); + BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); + + p = Vec3s(0, 0, 1.5); + res = Project::projectTriangle(v1, v2, v3, p); + BOOST_CHECK(res.encode == 1); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 1)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + + p = Vec3s(1.5, 0, 0); + res = Project::projectTriangle(v1, v2, v3, p); + BOOST_CHECK(res.encode == 4); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 1)); + + p = Vec3s(0, 1.5, 0); + res = Project::projectTriangle(v1, v2, v3, p); + BOOST_CHECK(res.encode == 2); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 1)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + + p = Vec3s(1, 1, 0); + res = Project::projectTriangle(v1, v2, v3, p); + BOOST_CHECK(res.encode == 6); + BOOST_CHECK(approx(res.sqr_distance, 0.5)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 0.5)); + BOOST_CHECK(approx(res.parameterization[2], 0.5)); + + p = Vec3s(1, 0, 1); + res = Project::projectTriangle(v1, v2, v3, p); + BOOST_CHECK(res.encode == 5); + BOOST_CHECK(approx(res.sqr_distance, 0.5)); + BOOST_CHECK(approx(res.parameterization[0], 0.5)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 0.5)); + + p = Vec3s(0, 1, 1); + res = Project::projectTriangle(v1, v2, v3, p); + BOOST_CHECK(res.encode == 3); + BOOST_CHECK(approx(res.sqr_distance, 0.5)); + BOOST_CHECK(approx(res.parameterization[0], 0.5)); + BOOST_CHECK(approx(res.parameterization[1], 0.5)); + BOOST_CHECK(approx(res.parameterization[2], 0)); +} + +BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { + Vec3s v1(0, 0, 1); + Vec3s v2(0, 1, 0); + Vec3s v3(1, 0, 0); + Vec3s v4(1, 1, 1); + + Vec3s p(0.5, 0.5, 0.5); + Project::ProjectResult res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 15); + BOOST_CHECK(approx(res.sqr_distance, 0)); + BOOST_CHECK(approx(res.parameterization[0], 0.25)); + BOOST_CHECK(approx(res.parameterization[1], 0.25)); + BOOST_CHECK(approx(res.parameterization[2], 0.25)); + BOOST_CHECK(approx(res.parameterization[3], 0.25)); + + p = Vec3s(0, 0, 0); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 7); + BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[3], 0)); + + p = Vec3s(0, 1, 1); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 11); + BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); + + p = Vec3s(1, 1, 0); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 14); + BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); + + p = Vec3s(1, 0, 1); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 13); + BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); + + p = Vec3s(1.5, 1.5, 1.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 8); + BOOST_CHECK(approx(res.sqr_distance, 0.75)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + BOOST_CHECK(approx(res.parameterization[3], 1)); + + p = Vec3s(1.5, -0.5, -0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 4); + BOOST_CHECK(approx(res.sqr_distance, 0.75)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 1)); + BOOST_CHECK(approx(res.parameterization[3], 0)); + + p = Vec3s(-0.5, -0.5, 1.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 1); + BOOST_CHECK(approx(res.sqr_distance, 0.75)); + BOOST_CHECK(approx(res.parameterization[0], 1)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + BOOST_CHECK(approx(res.parameterization[3], 0)); + + p = Vec3s(-0.5, 1.5, -0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 2); + BOOST_CHECK(approx(res.sqr_distance, 0.75)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 1)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + BOOST_CHECK(approx(res.parameterization[3], 0)); + + p = Vec3s(0.5, -0.5, 0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 5); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 0.5)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 0.5)); + BOOST_CHECK(approx(res.parameterization[3], 0)); + + p = Vec3s(0.5, 1.5, 0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 10); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 0.5)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + BOOST_CHECK(approx(res.parameterization[3], 0.5)); + + p = Vec3s(1.5, 0.5, 0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 12); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 0.5)); + BOOST_CHECK(approx(res.parameterization[3], 0.5)); + + p = Vec3s(-0.5, 0.5, 0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 3); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 0.5)); + BOOST_CHECK(approx(res.parameterization[1], 0.5)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + BOOST_CHECK(approx(res.parameterization[3], 0)); + + p = Vec3s(0.5, 0.5, 1.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 9); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 0.5)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + BOOST_CHECK(approx(res.parameterization[3], 0.5)); + + p = Vec3s(0.5, 0.5, -0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 6); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 0.5)); + BOOST_CHECK(approx(res.parameterization[2], 0.5)); + BOOST_CHECK(approx(res.parameterization[3], 0)); +} diff --git a/test/swept_sphere_radius.cpp b/test/swept_sphere_radius.cpp index 3bf8f538e..26df2cca3 100644 --- a/test/swept_sphere_radius.cpp +++ b/test/swept_sphere_radius.cpp @@ -34,20 +34,20 @@ /** \author Louis Montaut */ -#define BOOST_TEST_MODULE SWEPT_SPHERE_RADIUS +#define BOOST_TEST_MODULE COAL_SWEPT_SPHERE_RADIUS #include -#include -#include +#include "coal/narrowphase/narrowphase.h" +#include "coal/collision_utility.h" -#include -#include -#include -#include +#include "coal/serialization/geometric_shapes.h" +#include "coal/serialization/convex.h" +#include "coal/serialization/transform.h" +#include "coal/serialization/archive.h" #include "utility.h" -using namespace hpp::fcl; +using namespace coal; NODE_TYPE node1_type; NODE_TYPE node2_type; @@ -58,7 +58,7 @@ int line; node2_type = shape2.getNodeType(); \ line = __LINE__ -#define HPP_FCL_CHECK(cond) \ +#define COAL_CHECK(cond) \ BOOST_CHECK_MESSAGE( \ cond, "from line " << line << ", for collision pair: " \ << get_node_type_name(node1_type) << " - " \ @@ -67,17 +67,17 @@ int line; << ", ssr2 = " << shape2.getSweptSphereRadius() \ << ": " #cond) -#define HPP_FCL_CHECK_VECTOR_CLOSE(v1, v2, tol) \ - EIGEN_VECTOR_IS_APPROX(v1, v2, tol); \ - HPP_FCL_CHECK(((v1) - (v2)).isZero(tol)) +#define COAL_CHECK_VECTOR_CLOSE(v1, v2, tol) \ + EIGEN_VECTOR_IS_APPROX(v1, v2, tol); \ + COAL_CHECK(((v1) - (v2)).isZero(tol)) -#define HPP_FCL_CHECK_REAL_CLOSE(v1, v2, tol) \ - FCL_REAL_IS_APPROX(v1, v2, tol); \ - HPP_FCL_CHECK(std::abs((v1) - (v2)) < tol) +#define COAL_CHECK_REAL_CLOSE(v1, v2, tol) \ + CoalScalar_IS_APPROX(v1, v2, tol); \ + COAL_CHECK(std::abs((v1) - (v2)) < tol) -#define HPP_FCL_CHECK_CONDITION(cond) \ - BOOST_CHECK(cond); \ - HPP_FCL_CHECK(cond) +#define COAL_CHECK_CONDITION(cond) \ + BOOST_CHECK(cond); \ + COAL_CHECK(cond) // Preambule: swept sphere radius allows to virually convolve geometric shapes // by a sphere with positive radius (Minkowski sum). @@ -99,7 +99,7 @@ int line; // So we can also easily recover the witness points of the swept sphere shapes. // // This suite of test is designed to verify that property and generally test for -// swept-sphere radius support in hpp-fcl. +// swept-sphere radius support in Coal. // Notes: // - not all collision pairs use GJK/EPA, so this test makes sure that // swept-sphere radius is taken into account even for specialized collision @@ -113,19 +113,19 @@ int line; struct SweptSphereGJKSolver : public GJKSolver { template - FCL_REAL shapeDistance( - const S1& s1, const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, bool compute_penetration, Vec3f& p1, Vec3f& p2, - Vec3f& normal, bool use_swept_sphere_radius_in_gjk_epa_iterations) const { + CoalScalar shapeDistance( + const S1& s1, const Transform3s& tf1, const S2& s2, + const Transform3s& tf2, bool compute_penetration, Vec3s& p1, Vec3s& p2, + Vec3s& normal, bool use_swept_sphere_radius_in_gjk_epa_iterations) const { if (use_swept_sphere_radius_in_gjk_epa_iterations) { - FCL_REAL distance; + CoalScalar distance; this->runGJKAndEPA( s1, tf1, s2, tf2, compute_penetration, distance, p1, p2, normal); return distance; } - // Default behavior of hppfcl's GJKSolver - FCL_REAL distance; + // Default behavior of coal's GJKSolver + CoalScalar distance; this->runGJKAndEPA( s1, tf1, s2, tf2, compute_penetration, distance, p1, p2, normal); return distance; @@ -138,36 +138,36 @@ void test_gjksolver_swept_sphere_radius(S1& shape1, S2& shape2) { // The swept sphere radius is detrimental to the convergence of GJK // and EPA. This gets worse as the radius of the swept sphere increases. // So we need to increase the number of iterations to get a good result. - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; solver.gjk_tolerance = tol; solver.epa_tolerance = tol; solver.epa_max_iterations = 1000; const bool compute_penetration = true; - FCL_REAL extents[] = {-2, -2, -2, 2, 2, 2}; + CoalScalar extents[] = {-2, -2, -2, 2, 2, 2}; std::size_t n = 10; - std::vector tf1s; - std::vector tf2s; + std::vector tf1s; + std::vector tf2s; generateRandomTransforms(extents, tf1s, n); generateRandomTransforms(extents, tf2s, n); - const std::array swept_sphere_radius = {0, 0.1, 1., 10.}; + const std::array swept_sphere_radius = {0, 0.1, 1., 10.}; - for (const FCL_REAL& ssr1 : swept_sphere_radius) { + for (const CoalScalar& ssr1 : swept_sphere_radius) { shape1.setSweptSphereRadius(ssr1); - for (const FCL_REAL& ssr2 : swept_sphere_radius) { + for (const CoalScalar& ssr2 : swept_sphere_radius) { shape2.setSweptSphereRadius(ssr2); for (std::size_t i = 0; i < n; ++i) { - Transform3f tf1 = tf1s[i]; - Transform3f tf2 = tf2s[i]; + Transform3s tf1 = tf1s[i]; + Transform3s tf2 = tf2s[i]; SET_LINE; - std::array distance; - std::array p1; - std::array p2; - std::array normal; + std::array distance; + std::array p1; + std::array p2; + std::array normal; - // Default hppfcl behavior - Don't take swept sphere radius into account + // Default coal behavior - Don't take swept sphere radius into account // during GJK/EPA iterations. Correct the solution afterwards. distance[0] = solver.shapeDistance(shape1, tf1, shape2, tf2, compute_penetration, @@ -180,31 +180,31 @@ void test_gjksolver_swept_sphere_radius(S1& shape1, S2& shape2) { // Precision is dependent on the swept-sphere radius. // The issue of precision does not come from the default behavior of - // hppfcl, but from the result in which we manually take the swept + // coal, but from the result in which we manually take the swept // sphere radius into account in GJK/EPA iterations. - const FCL_REAL precision = + const CoalScalar precision = 3 * sqrt(tol) + (1 / 100.0) * std::max(shape1.getSweptSphereRadius(), shape2.getSweptSphereRadius()); // Check that the distance is the same - HPP_FCL_CHECK_REAL_CLOSE(distance[0], distance[1], precision); + COAL_CHECK_REAL_CLOSE(distance[0], distance[1], precision); // Check that the normal is the same - HPP_FCL_CHECK_CONDITION(normal[0].dot(normal[1]) > 0); - HPP_FCL_CHECK_CONDITION(std::abs(1 - normal[0].dot(normal[1])) < - precision); + COAL_CHECK_CONDITION(normal[0].dot(normal[1]) > 0); + COAL_CHECK_CONDITION(std::abs(1 - normal[0].dot(normal[1])) < + precision); // Check that the witness points are the same - HPP_FCL_CHECK_VECTOR_CLOSE(p1[0], p1[1], precision); - HPP_FCL_CHECK_VECTOR_CLOSE(p2[0], p2[1], precision); + COAL_CHECK_VECTOR_CLOSE(p1[0], p1[1], precision); + COAL_CHECK_VECTOR_CLOSE(p2[0], p2[1], precision); } } } } -static const FCL_REAL min_shape_size = 0.1; -static const FCL_REAL max_shape_size = 0.5; +static const CoalScalar min_shape_size = 0.1; +static const CoalScalar max_shape_size = 0.5; BOOST_AUTO_TEST_CASE(ssr_mesh_mesh) { Convex shape1 = makeRandomConvex(min_shape_size, max_shape_size); @@ -281,21 +281,21 @@ void test_collide_swept_sphere_radius(S1& shape1, S2& shape2) { << std::string(get_node_type_name(shape1.getNodeType())) << " and " << std::string(get_node_type_name(shape2.getNodeType())) << '\n'; - FCL_REAL extents[] = {-2, -2, -2, 2, 2, 2}; + CoalScalar extents[] = {-2, -2, -2, 2, 2, 2}; std::size_t n = 1; - std::vector tf1s; - std::vector tf2s; + std::vector tf1s; + std::vector tf2s; generateRandomTransforms(extents, tf1s, n); generateRandomTransforms(extents, tf2s, n); - const std::array swept_sphere_radius = {0, 0.1, 1., 10.}; - for (const FCL_REAL& ssr1 : swept_sphere_radius) { + const std::array swept_sphere_radius = {0, 0.1, 1., 10.}; + for (const CoalScalar& ssr1 : swept_sphere_radius) { shape1.setSweptSphereRadius(ssr1); - for (const FCL_REAL& ssr2 : swept_sphere_radius) { + for (const CoalScalar& ssr2 : swept_sphere_radius) { shape2.setSweptSphereRadius(ssr2); for (std::size_t i = 0; i < n; ++i) { - Transform3f tf1 = tf1s[i]; - Transform3f tf2 = tf2s[i]; + Transform3s tf1 = tf1s[i]; + Transform3s tf2 = tf2s[i]; SET_LINE; CollisionRequest request; @@ -303,24 +303,24 @@ void test_collide_swept_sphere_radius(S1& shape1, S2& shape2) { // We make sure we get witness points by setting the security margin to // infinity. That way shape1 and shape2 will always be considered in // collision. - request.security_margin = std::numeric_limits::max(); - const FCL_REAL tol = 1e-6; + request.security_margin = std::numeric_limits::max(); + const CoalScalar tol = 1e-6; request.gjk_tolerance = tol; request.epa_tolerance = tol; std::array result; // Without swept sphere radius - const FCL_REAL ssr1 = shape1.getSweptSphereRadius(); - const FCL_REAL ssr2 = shape2.getSweptSphereRadius(); + const CoalScalar ssr1 = shape1.getSweptSphereRadius(); + const CoalScalar ssr2 = shape2.getSweptSphereRadius(); shape1.setSweptSphereRadius(0.); shape2.setSweptSphereRadius(0.); - hpp::fcl::collide(&shape1, tf1, &shape2, tf2, request, result[0]); + coal::collide(&shape1, tf1, &shape2, tf2, request, result[0]); // With swept sphere radius shape1.setSweptSphereRadius(ssr1); shape2.setSweptSphereRadius(ssr2); - hpp::fcl::collide(&shape1, tf1, &shape2, tf2, request, result[1]); + coal::collide(&shape1, tf1, &shape2, tf2, request, result[1]); BOOST_CHECK(result[0].isCollision()); BOOST_CHECK(result[1].isCollision()); @@ -331,28 +331,27 @@ void test_collide_swept_sphere_radius(S1& shape1, S2& shape2) { // Precision is dependent on the swept sphere radii. // The issue of precision does not come from the default behavior of - // hppfcl, but from the result in which we manually take the swept + // coal, but from the result in which we manually take the swept // sphere radius into account in GJK/EPA iterations. - const FCL_REAL precision = + const CoalScalar precision = 3 * sqrt(tol) + (1 / 100.0) * std::max(ssr1, ssr2); - const FCL_REAL ssr = ssr1 + ssr2; + const CoalScalar ssr = ssr1 + ssr2; // Check that the distance is the same - HPP_FCL_CHECK_REAL_CLOSE(contact[0].penetration_depth - ssr, - contact[1].penetration_depth, precision); + COAL_CHECK_REAL_CLOSE(contact[0].penetration_depth - ssr, + contact[1].penetration_depth, precision); // Check that the normal is the same - HPP_FCL_CHECK_CONDITION((contact[0].normal).dot(contact[1].normal) > - 0); - HPP_FCL_CHECK_CONDITION( + COAL_CHECK_CONDITION((contact[0].normal).dot(contact[1].normal) > 0); + COAL_CHECK_CONDITION( std::abs(1 - (contact[0].normal).dot(contact[1].normal)) < precision); // Check that the witness points are the same - HPP_FCL_CHECK_VECTOR_CLOSE( + COAL_CHECK_VECTOR_CLOSE( contact[0].nearest_points[0] + ssr1 * contact[0].normal, contact[1].nearest_points[0], precision); - HPP_FCL_CHECK_VECTOR_CLOSE( + COAL_CHECK_VECTOR_CLOSE( contact[0].nearest_points[1] - ssr2 * contact[0].normal, contact[1].nearest_points[1], precision); } diff --git a/test/utility.cpp b/test/utility.cpp index cf20cdd14..714469f01 100644 --- a/test/utility.cpp +++ b/test/utility.cpp @@ -1,21 +1,20 @@ #include "utility.h" -#include -#include -#include -#include -#include +#include "coal/BV/BV.h" +#include "coal/BVH/BVH_model.h" +#include "coal/shape/geometric_shape_to_BVH_model.h" +#include "coal/collision_utility.h" +#include "coal/fwd.hh" -#include -#include +#include "coal/collision.h" +#include "coal/distance.h" #include #include #include #include -namespace hpp { -namespace fcl { +namespace coal { BenchTimer::BenchTimer() { #ifdef _WIN32 @@ -86,16 +85,16 @@ const Eigen::IOFormat vfmt = Eigen::IOFormat( const Eigen::IOFormat pyfmt = Eigen::IOFormat( Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]"); -const Vec3f UnitX = Vec3f(1, 0, 0); -const Vec3f UnitY = Vec3f(0, 1, 0); -const Vec3f UnitZ = Vec3f(0, 0, 1); +const Vec3s UnitX = Vec3s(1, 0, 0); +const Vec3s UnitY = Vec3s(0, 1, 0); +const Vec3s UnitZ = Vec3s(0, 0, 1); -FCL_REAL rand_interval(FCL_REAL rmin, FCL_REAL rmax) { - FCL_REAL t = rand() / ((FCL_REAL)RAND_MAX + 1); +CoalScalar rand_interval(CoalScalar rmin, CoalScalar rmax) { + CoalScalar t = rand() / ((CoalScalar)RAND_MAX + 1); return (t * (rmax - rmin) + rmin); } -void loadOBJFile(const char* filename, std::vector& points, +void loadOBJFile(const char* filename, std::vector& points, std::vector& triangles) { FILE* file = fopen(filename, "rb"); if (!file) { @@ -122,10 +121,10 @@ void loadOBJFile(const char* filename, std::vector& points, strtok(NULL, "\t "); has_texture = true; } else { - FCL_REAL x = (FCL_REAL)atof(strtok(NULL, "\t ")); - FCL_REAL y = (FCL_REAL)atof(strtok(NULL, "\t ")); - FCL_REAL z = (FCL_REAL)atof(strtok(NULL, "\t ")); - Vec3f p(x, y, z); + CoalScalar x = (CoalScalar)atof(strtok(NULL, "\t ")); + CoalScalar y = (CoalScalar)atof(strtok(NULL, "\t ")); + CoalScalar z = (CoalScalar)atof(strtok(NULL, "\t ")); + Vec3s p(x, y, z); points.push_back(p); } } break; @@ -161,7 +160,7 @@ void loadOBJFile(const char* filename, std::vector& points, } } -void saveOBJFile(const char* filename, std::vector& points, +void saveOBJFile(const char* filename, std::vector& points, std::vector& triangles) { std::ofstream os(filename); if (!os) { @@ -182,8 +181,9 @@ void saveOBJFile(const char* filename, std::vector& points, os.close(); } -#ifdef HPP_FCL_HAS_OCTOMAP -OcTree loadOctreeFile(const std::string& filename, const FCL_REAL& resolution) { +#ifdef COAL_HAS_OCTOMAP +OcTree loadOctreeFile(const std::string& filename, + const CoalScalar& resolution) { octomap::OcTreePtr_t octree(new octomap::OcTree(filename)); if (octree->getResolution() != resolution) { std::ostringstream oss; @@ -191,97 +191,97 @@ OcTree loadOctreeFile(const std::string& filename, const FCL_REAL& resolution) { << " and not " << resolution; throw std::invalid_argument(oss.str()); } - return hpp::fcl::OcTree(octree); + return coal::OcTree(octree); } #endif -void eulerToMatrix(FCL_REAL a, FCL_REAL b, FCL_REAL c, Matrix3f& R) { - FCL_REAL c1 = cos(a); - FCL_REAL c2 = cos(b); - FCL_REAL c3 = cos(c); - FCL_REAL s1 = sin(a); - FCL_REAL s2 = sin(b); - FCL_REAL s3 = sin(c); +void eulerToMatrix(CoalScalar a, CoalScalar b, CoalScalar c, Matrix3s& R) { + CoalScalar c1 = cos(a); + CoalScalar c2 = cos(b); + CoalScalar c3 = cos(c); + CoalScalar s1 = sin(a); + CoalScalar s2 = sin(b); + CoalScalar s3 = sin(c); R << c1 * c2, -c2 * s1, s2, c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3, -c2 * s3, s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3; } -void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform) { - FCL_REAL x = rand_interval(extents[0], extents[3]); - FCL_REAL y = rand_interval(extents[1], extents[4]); - FCL_REAL z = rand_interval(extents[2], extents[5]); +void generateRandomTransform(CoalScalar extents[6], Transform3s& transform) { + CoalScalar x = rand_interval(extents[0], extents[3]); + CoalScalar y = rand_interval(extents[1], extents[4]); + CoalScalar z = rand_interval(extents[2], extents[5]); - const FCL_REAL pi = 3.1415926; - FCL_REAL a = rand_interval(0, 2 * pi); - FCL_REAL b = rand_interval(0, 2 * pi); - FCL_REAL c = rand_interval(0, 2 * pi); + const CoalScalar pi = 3.1415926; + CoalScalar a = rand_interval(0, 2 * pi); + CoalScalar b = rand_interval(0, 2 * pi); + CoalScalar c = rand_interval(0, 2 * pi); - Matrix3f R; + Matrix3s R; eulerToMatrix(a, b, c, R); - Vec3f T(x, y, z); + Vec3s T(x, y, z); transform.setTransform(R, T); } -void generateRandomTransforms(FCL_REAL extents[6], - std::vector& transforms, +void generateRandomTransforms(CoalScalar extents[6], + std::vector& transforms, std::size_t n) { transforms.resize(n); for (std::size_t i = 0; i < n; ++i) { - FCL_REAL x = rand_interval(extents[0], extents[3]); - FCL_REAL y = rand_interval(extents[1], extents[4]); - FCL_REAL z = rand_interval(extents[2], extents[5]); + CoalScalar x = rand_interval(extents[0], extents[3]); + CoalScalar y = rand_interval(extents[1], extents[4]); + CoalScalar z = rand_interval(extents[2], extents[5]); - const FCL_REAL pi = 3.1415926; - FCL_REAL a = rand_interval(0, 2 * pi); - FCL_REAL b = rand_interval(0, 2 * pi); - FCL_REAL c = rand_interval(0, 2 * pi); + const CoalScalar pi = 3.1415926; + CoalScalar a = rand_interval(0, 2 * pi); + CoalScalar b = rand_interval(0, 2 * pi); + CoalScalar c = rand_interval(0, 2 * pi); { - Matrix3f R; + Matrix3s R; eulerToMatrix(a, b, c, R); - Vec3f T(x, y, z); + Vec3s T(x, y, z); transforms[i].setTransform(R, T); } } } -void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], - FCL_REAL delta_rot, - std::vector& transforms, - std::vector& transforms2, +void generateRandomTransforms(CoalScalar extents[6], CoalScalar delta_trans[3], + CoalScalar delta_rot, + std::vector& transforms, + std::vector& transforms2, std::size_t n) { transforms.resize(n); transforms2.resize(n); for (std::size_t i = 0; i < n; ++i) { - FCL_REAL x = rand_interval(extents[0], extents[3]); - FCL_REAL y = rand_interval(extents[1], extents[4]); - FCL_REAL z = rand_interval(extents[2], extents[5]); + CoalScalar x = rand_interval(extents[0], extents[3]); + CoalScalar y = rand_interval(extents[1], extents[4]); + CoalScalar z = rand_interval(extents[2], extents[5]); - const FCL_REAL pi = 3.1415926; - FCL_REAL a = rand_interval(0, 2 * pi); - FCL_REAL b = rand_interval(0, 2 * pi); - FCL_REAL c = rand_interval(0, 2 * pi); + const CoalScalar pi = 3.1415926; + CoalScalar a = rand_interval(0, 2 * pi); + CoalScalar b = rand_interval(0, 2 * pi); + CoalScalar c = rand_interval(0, 2 * pi); { - Matrix3f R; + Matrix3s R; eulerToMatrix(a, b, c, R); - Vec3f T(x, y, z); + Vec3s T(x, y, z); transforms[i].setTransform(R, T); } - FCL_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]); - FCL_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]); - FCL_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]); + CoalScalar deltax = rand_interval(-delta_trans[0], delta_trans[0]); + CoalScalar deltay = rand_interval(-delta_trans[1], delta_trans[1]); + CoalScalar deltaz = rand_interval(-delta_trans[2], delta_trans[2]); - FCL_REAL deltaa = rand_interval(-delta_rot, delta_rot); - FCL_REAL deltab = rand_interval(-delta_rot, delta_rot); - FCL_REAL deltac = rand_interval(-delta_rot, delta_rot); + CoalScalar deltaa = rand_interval(-delta_rot, delta_rot); + CoalScalar deltab = rand_interval(-delta_rot, delta_rot); + CoalScalar deltac = rand_interval(-delta_rot, delta_rot); { - Matrix3f R; + Matrix3s R; eulerToMatrix(a + deltaa, b + deltab, c + deltac, R); - Vec3f T(x + deltax, y + deltay, z + deltaz); + Vec3s T(x + deltax, y + deltay, z + deltaz); transforms2[i].setTransform(R, T); } } @@ -305,7 +305,7 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, } bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, - void* cdata_, FCL_REAL& dist) { + void* cdata_, CoalScalar& dist) { DistanceData* cdata = static_cast(cdata_); const DistanceRequest& request = cdata->request; DistanceResult& result = cdata->result; @@ -367,7 +367,7 @@ std::string getNodeTypeName(NODE_TYPE node_type) { return std::string("invalid"); } -Quatf makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z) { +Quatf makeQuat(CoalScalar w, CoalScalar x, CoalScalar y, CoalScalar z) { Quatf q; q.w() = w; q.x() = x; @@ -376,7 +376,7 @@ Quatf makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z) { return q; } -std::ostream& operator<<(std::ostream& os, const Transform3f& tf) { +std::ostream& operator<<(std::ostream& os, const Transform3s& tf) { return os << "[ " << tf.getTranslation().format(vfmt) << ", " << tf.getQuatRotation().coeffs().format(vfmt) << " ]"; } @@ -390,10 +390,10 @@ std::size_t getNbRun(const int& argc, char const* const* argv, } void generateEnvironments(std::vector& env, - FCL_REAL env_scale, std::size_t n) { - FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, - env_scale, -env_scale, env_scale}; - std::vector transforms(n); + CoalScalar env_scale, std::size_t n) { + CoalScalar extents[] = {-env_scale, env_scale, -env_scale, + env_scale, -env_scale, env_scale}; + std::vector transforms(n); generateRandomTransforms(extents, transforms, n); for (std::size_t i = 0; i < n; ++i) { @@ -421,16 +421,16 @@ void generateEnvironments(std::vector& env, } void generateEnvironmentsMesh(std::vector& env, - FCL_REAL env_scale, std::size_t n) { - FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, - env_scale, -env_scale, env_scale}; - std::vector transforms; + CoalScalar env_scale, std::size_t n) { + CoalScalar extents[] = {-env_scale, env_scale, -env_scale, + env_scale, -env_scale, env_scale}; + std::vector transforms; generateRandomTransforms(extents, transforms, n); Box box(5, 10, 20); for (std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); - generateBVHModel(*model, box, Transform3f::Identity()); + generateBVHModel(*model, box, Transform3s::Identity()); env.push_back(new CollisionObject(shared_ptr(model), transforms[i])); env.back()->collisionGeometry()->computeLocalAABB(); @@ -440,7 +440,7 @@ void generateEnvironmentsMesh(std::vector& env, Sphere sphere(30); for (std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); - generateBVHModel(*model, sphere, Transform3f::Identity(), 16, 16); + generateBVHModel(*model, sphere, Transform3s::Identity(), 16, 16); env.push_back(new CollisionObject(shared_ptr(model), transforms[i])); env.back()->collisionGeometry()->computeLocalAABB(); @@ -450,18 +450,18 @@ void generateEnvironmentsMesh(std::vector& env, Cylinder cylinder(10, 40); for (std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); - generateBVHModel(*model, cylinder, Transform3f::Identity(), 16, 16); + generateBVHModel(*model, cylinder, Transform3s::Identity(), 16, 16); env.push_back(new CollisionObject(shared_ptr(model), transforms[i])); env.back()->collisionGeometry()->computeLocalAABB(); } } -Convex buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d) { - std::shared_ptr> pts(new std::vector( - {Vec3f(l, w, d), Vec3f(l, w, -d), Vec3f(l, -w, d), Vec3f(l, -w, -d), - Vec3f(-l, w, d), Vec3f(-l, w, -d), Vec3f(-l, -w, d), - Vec3f(-l, -w, -d)})); +Convex buildBox(CoalScalar l, CoalScalar w, CoalScalar d) { + std::shared_ptr> pts(new std::vector( + {Vec3s(l, w, d), Vec3s(l, w, -d), Vec3s(l, -w, d), Vec3s(l, -w, -d), + Vec3s(-l, w, d), Vec3s(-l, w, -d), Vec3s(-l, -w, d), + Vec3s(-l, -w, -d)})); std::shared_ptr> polygons( new std::vector(6)); @@ -480,7 +480,7 @@ Convex buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d) { } /// Takes a point and projects it onto the surface of the unit sphere -void toSphere(Vec3f& point) { +void toSphere(Vec3s& point) { assert(point.norm() > 1e-8); point /= point.norm(); } @@ -491,7 +491,7 @@ void toSphere(Vec3f& point) { /// ellipsoid. Thus, the point y = A^(1/2) * x belongs to the unit sphere if y * /// y = 1. Therefore, the tranformation which brings y to x is A^(-1/2) = /// diag(r). -void toEllipsoid(Vec3f& point, const Ellipsoid& ellipsoid) { +void toEllipsoid(Vec3s& point, const Ellipsoid& ellipsoid) { toSphere(point); point[0] *= ellipsoid.radii[0]; point[1] *= ellipsoid.radii[1]; @@ -499,27 +499,27 @@ void toEllipsoid(Vec3f& point, const Ellipsoid& ellipsoid) { } Convex constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid) { - FCL_REAL PHI = (1 + std::sqrt(5)) / 2; + CoalScalar PHI = (1 + std::sqrt(5)) / 2; // vertices - std::shared_ptr> pts(new std::vector({ - Vec3f(-1, PHI, 0), - Vec3f(1, PHI, 0), - Vec3f(-1, -PHI, 0), - Vec3f(1, -PHI, 0), - - Vec3f(0, -1, PHI), - Vec3f(0, 1, PHI), - Vec3f(0, -1, -PHI), - Vec3f(0, 1, -PHI), - - Vec3f(PHI, 0, -1), - Vec3f(PHI, 0, 1), - Vec3f(-PHI, 0, -1), - Vec3f(-PHI, 0, 1), + std::shared_ptr> pts(new std::vector({ + Vec3s(-1, PHI, 0), + Vec3s(1, PHI, 0), + Vec3s(-1, -PHI, 0), + Vec3s(1, -PHI, 0), + + Vec3s(0, -1, PHI), + Vec3s(0, 1, PHI), + Vec3s(0, -1, -PHI), + Vec3s(0, 1, -PHI), + + Vec3s(PHI, 0, -1), + Vec3s(PHI, 0, 1), + Vec3s(-PHI, 0, -1), + Vec3s(-PHI, 0, 1), })); - std::vector& pts_ = *pts; + std::vector& pts_ = *pts; for (size_t i = 0; i < 12; ++i) { toEllipsoid(pts_[i], ellipsoid); } @@ -556,61 +556,61 @@ Convex constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid) { ); } -Box makeRandomBox(FCL_REAL min_size, FCL_REAL max_size) { - return Box(Vec3f(rand_interval(min_size, max_size), +Box makeRandomBox(CoalScalar min_size, CoalScalar max_size) { + return Box(Vec3s(rand_interval(min_size, max_size), rand_interval(min_size, max_size), rand_interval(min_size, max_size))); } -Sphere makeRandomSphere(FCL_REAL min_size, FCL_REAL max_size) { +Sphere makeRandomSphere(CoalScalar min_size, CoalScalar max_size) { return Sphere(rand_interval(min_size, max_size)); } -Ellipsoid makeRandomEllipsoid(FCL_REAL min_size, FCL_REAL max_size) { - return Ellipsoid(Vec3f(rand_interval(min_size, max_size), +Ellipsoid makeRandomEllipsoid(CoalScalar min_size, CoalScalar max_size) { + return Ellipsoid(Vec3s(rand_interval(min_size, max_size), rand_interval(min_size, max_size), rand_interval(min_size, max_size))); } -Capsule makeRandomCapsule(std::array min_size, - std::array max_size) { +Capsule makeRandomCapsule(std::array min_size, + std::array max_size) { return Capsule(rand_interval(min_size[0], max_size[0]), rand_interval(min_size[1], max_size[1])); } -Cone makeRandomCone(std::array min_size, - std::array max_size) { +Cone makeRandomCone(std::array min_size, + std::array max_size) { return Cone(rand_interval(min_size[0], max_size[0]), rand_interval(min_size[1], max_size[1])); } -Cylinder makeRandomCylinder(std::array min_size, - std::array max_size) { +Cylinder makeRandomCylinder(std::array min_size, + std::array max_size) { return Cylinder(rand_interval(min_size[0], max_size[0]), rand_interval(min_size[1], max_size[1])); } -Convex makeRandomConvex(FCL_REAL min_size, FCL_REAL max_size) { +Convex makeRandomConvex(CoalScalar min_size, CoalScalar max_size) { Ellipsoid ellipsoid = makeRandomEllipsoid(min_size, max_size); return constructPolytopeFromEllipsoid(ellipsoid); } -Plane makeRandomPlane(FCL_REAL min_size, FCL_REAL max_size) { - return Plane(Vec3f::Random().normalized(), rand_interval(min_size, max_size)); +Plane makeRandomPlane(CoalScalar min_size, CoalScalar max_size) { + return Plane(Vec3s::Random().normalized(), rand_interval(min_size, max_size)); } -Halfspace makeRandomHalfspace(FCL_REAL min_size, FCL_REAL max_size) { - return Halfspace(Vec3f::Random().normalized(), +Halfspace makeRandomHalfspace(CoalScalar min_size, CoalScalar max_size) { + return Halfspace(Vec3s::Random().normalized(), rand_interval(min_size, max_size)); } std::shared_ptr makeRandomGeometry(NODE_TYPE node_type) { switch (node_type) { case GEOM_TRIANGLE: - HPP_FCL_THROW_PRETTY(std::string(HPP_FCL_PRETTY_FUNCTION) + " for " + - std::string(get_node_type_name(node_type)) + - " is not yet implemented.", - std::invalid_argument); + COAL_THROW_PRETTY(std::string(COAL_PRETTY_FUNCTION) + " for " + + std::string(get_node_type_name(node_type)) + + " is not yet implemented.", + std::invalid_argument); break; case GEOM_BOX: return std::make_shared(makeRandomBox(0.1, 1.0)); @@ -642,13 +642,11 @@ std::shared_ptr makeRandomGeometry(NODE_TYPE node_type) { return std::make_shared(makeRandomHalfspace(0.1, 1.0)); break; default: - HPP_FCL_THROW_PRETTY(std::string(get_node_type_name(node_type)) + - " is not a primitive shape.", - std::invalid_argument); + COAL_THROW_PRETTY(std::string(get_node_type_name(node_type)) + + " is not a primitive shape.", + std::invalid_argument); break; } } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/test/utility.h b/test/utility.h index b81723ab9..6be386e5e 100644 --- a/test/utility.h +++ b/test/utility.h @@ -35,17 +35,17 @@ /** \author Jia Pan */ -#ifndef TEST_HPP_FCL_UTILITY_H -#define TEST_HPP_FCL_UTILITY_H +#ifndef TEST_COAL_UTILITY_H +#define TEST_COAL_UTILITY_H -#include -#include -#include -#include -#include +#include "coal/math/transform.h" +#include "coal/collision_data.h" +#include "coal/collision_object.h" +#include "coal/broadphase/default_broadphase_callbacks.h" +#include "coal/shape/convex.h" -#ifdef HPP_FCL_HAS_OCTOMAP -#include +#ifdef COAL_HAS_OCTOMAP +#include "coal/octree.h" #endif #ifdef _WIN32 @@ -70,7 +70,7 @@ << (Va) << "\n!=\n" \ << (Vb) << "\n]") -#define FCL_REAL_IS_APPROX(Va, Vb, precision) \ +#define CoalScalar_IS_APPROX(Va, Vb, precision) \ BOOST_CHECK_MESSAGE(std::abs((Va) - (Vb)) < precision, \ "check " #Va ".isApprox(" #Vb ") failed at precision " \ << precision << " [\n" \ @@ -78,12 +78,11 @@ << Vb << "\n]") namespace octomap { -#ifdef HPP_FCL_HAS_OCTOMAP -typedef hpp::fcl::shared_ptr OcTreePtr_t; +#ifdef COAL_HAS_OCTOMAP +typedef coal::shared_ptr OcTreePtr_t; #endif } // namespace octomap -namespace hpp { -namespace fcl { +namespace coal { class BenchTimer { public: @@ -126,50 +125,50 @@ struct TStruct { extern const Eigen::IOFormat vfmt; extern const Eigen::IOFormat pyfmt; -typedef Eigen::AngleAxis AngleAxis; -extern const Vec3f UnitX; -extern const Vec3f UnitY; -extern const Vec3f UnitZ; +typedef Eigen::AngleAxis AngleAxis; +extern const Vec3s UnitX; +extern const Vec3s UnitY; +extern const Vec3s UnitZ; /// @brief Load an obj mesh file -void loadOBJFile(const char* filename, std::vector& points, +void loadOBJFile(const char* filename, std::vector& points, std::vector& triangles); -void saveOBJFile(const char* filename, std::vector& points, +void saveOBJFile(const char* filename, std::vector& points, std::vector& triangles); -#ifdef HPP_FCL_HAS_OCTOMAP -fcl::OcTree loadOctreeFile(const std::string& filename, - const FCL_REAL& resolution); +#ifdef COAL_HAS_OCTOMAP +coal::OcTree loadOctreeFile(const std::string& filename, + const CoalScalar& resolution); #endif /// @brief Generate one random transform whose translation is constrained by /// extents and rotation without constraints. The translation is (x, y, z), and /// extents[0] <= x <= extents[3], extents[1] <= y <= extents[4], extents[2] <= /// z <= extents[5] -void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform); +void generateRandomTransform(CoalScalar extents[6], Transform3s& transform); /// @brief Generate n random transforms whose translations are constrained by /// extents. -void generateRandomTransforms(FCL_REAL extents[6], - std::vector& transforms, +void generateRandomTransforms(CoalScalar extents[6], + std::vector& transforms, std::size_t n); /// @brief Generate n random transforms whose translations are constrained by /// extents. Also generate another transforms2 which have additional random /// translation & rotation to the transforms generated. -void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], - FCL_REAL delta_rot, - std::vector& transforms, - std::vector& transforms2, +void generateRandomTransforms(CoalScalar extents[6], CoalScalar delta_trans[3], + CoalScalar delta_rot, + std::vector& transforms, + std::vector& transforms2, std::size_t n); /// @ brief Structure for minimum distance between two meshes and the /// corresponding nearest point pair struct DistanceRes { double distance; - Vec3f p1; - Vec3f p2; + Vec3s p1; + Vec3s p2; }; /// @brief Default collision callback for two objects o1 and o2 in broad phase. @@ -181,28 +180,28 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, /// return value means whether the broad phase can stop now. also return dist, /// i.e. the bmin distance till now bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, - void* cdata, FCL_REAL& dist); + void* cdata, CoalScalar& dist); std::string getNodeTypeName(NODE_TYPE node_type); -Quatf makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z); +Quatf makeQuat(CoalScalar w, CoalScalar x, CoalScalar y, CoalScalar z); -std::ostream& operator<<(std::ostream& os, const Transform3f& tf); +std::ostream& operator<<(std::ostream& os, const Transform3s& tf); /// Get the argument --nb-run from argv std::size_t getNbRun(const int& argc, char const* const* argv, std::size_t defaultValue); void generateEnvironments(std::vector& env, - FCL_REAL env_scale, std::size_t n); + CoalScalar env_scale, std::size_t n); void generateEnvironmentsMesh(std::vector& env, - FCL_REAL env_scale, std::size_t n); + CoalScalar env_scale, std::size_t n); /// @brief Constructs a box with halfsides (l, w, d), centered around 0. /// The box is 2*l wide on the x-axis, 2*w wide on the y-axis and 2*d wide on /// the z-axis. -Convex buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d); +Convex buildBox(CoalScalar l, CoalScalar w, CoalScalar d); /// @brief We give an ellipsoid as input. The output is a 20 faces polytope /// which vertices belong to the original ellipsoid surface. The procedure is @@ -211,31 +210,29 @@ Convex buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d); /// ellipsoid tranformation to each vertex of the icosahedron. Convex constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid); -Box makeRandomBox(FCL_REAL min_size, FCL_REAL max_size); +Box makeRandomBox(CoalScalar min_size, CoalScalar max_size); -Sphere makeRandomSphere(FCL_REAL min_size, FCL_REAL max_size); +Sphere makeRandomSphere(CoalScalar min_size, CoalScalar max_size); -Ellipsoid makeRandomEllipsoid(FCL_REAL min_size, FCL_REAL max_size); +Ellipsoid makeRandomEllipsoid(CoalScalar min_size, CoalScalar max_size); -Capsule makeRandomCapsule(std::array min_size, - std::array max_size); +Capsule makeRandomCapsule(std::array min_size, + std::array max_size); -Cone makeRandomCone(std::array min_size, - std::array max_size); +Cone makeRandomCone(std::array min_size, + std::array max_size); -Cylinder makeRandomCylinder(std::array min_size, - std::array max_size); +Cylinder makeRandomCylinder(std::array min_size, + std::array max_size); -Convex makeRandomConvex(FCL_REAL min_size, FCL_REAL max_size); +Convex makeRandomConvex(CoalScalar min_size, CoalScalar max_size); -Plane makeRandomPlane(FCL_REAL min_size, FCL_REAL max_size); +Plane makeRandomPlane(CoalScalar min_size, CoalScalar max_size); -Halfspace makeRandomHalfspace(FCL_REAL min_size, FCL_REAL max_size); +Halfspace makeRandomHalfspace(CoalScalar min_size, CoalScalar max_size); std::shared_ptr makeRandomGeometry(NODE_TYPE node_type); -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif