diff --git a/.appveyor.yml b/.appveyor.yml index 18488edca..4131300de 100644 --- a/.appveyor.yml +++ b/.appveyor.yml @@ -57,4 +57,4 @@ build: parallel: true test_script: - - cmd: ctest -C %Configuration% + - cmd: ctest -C %Configuration% --output-on-failure diff --git a/.clang-format b/.clang-format new file mode 100644 index 000000000..5e53779da --- /dev/null +++ b/.clang-format @@ -0,0 +1,40 @@ +# -*- mode: yaml -*- +# vi: set ft=yaml : + +# Copyright (c) 2018, Toyota Research Institute, Inc. +# 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 HOLDER 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: Jamie Snape, Kitware, Inc. + +--- +BasedOnStyle: Google +--- +Language: Cpp +DerivePointerAlignment: false +PointerAlignment: Left diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 000000000..1fe616b07 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,36 @@ +# -*- mode: yaml -*- +# vi: set ft=yaml : + +# Copyright (c) 2018, Toyota Research Institute, Inc. +# 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 HOLDER 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: Jamie Snape, Kitware, Inc. + +--- +Checks: 'clang-analyzer-*,clang-diagnostic-*,cppcoreguidelines-*,google-*,modernize-*,performance-*,readability-*' diff --git a/.editorconfig b/.editorconfig new file mode 100644 index 000000000..69c4fcdc9 --- /dev/null +++ b/.editorconfig @@ -0,0 +1,44 @@ +# Copyright (c) 2018, Toyota Research Institute, Inc. +# 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 HOLDER 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: Jamie Snape, Kitware, Inc. + +root = true + +[*] +charset = utf +end_of_line = lf +indent_size = 2 +indent_style = space +insert_final_newline = true +max_line_length = 80 +trim_trailing_whitespace = true + +[*.md] +trim_trailing_whitespace = false diff --git a/.gitignore b/.gitignore index 1b386869a..6a8e562ef 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,34 @@ -doc/doxygen +# Copyright (c) 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 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 HOLDER 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: Jeongseok Lee + +/build +/build-* +/doc/doxygen diff --git a/.travis.yml b/.travis.yml index dd2697c75..13725ace4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -20,11 +20,11 @@ matrix: compiler: clang env: BUILD_TYPE=Release COVERALLS=OFF - os: osx - osx_image: xcode7.3 + osx_image: xcode9 compiler: clang env: BUILD_TYPE=Debug COVERALLS=OFF - os: osx - osx_image: xcode7.3 + osx_image: xcode9 compiler: clang env: BUILD_TYPE=Release COVERALLS=OFF @@ -46,13 +46,8 @@ script: - if [ $COVERALLS = ON ] && [ "$TRAVIS_OS_NAME" = "linux" ]; then make coveralls; fi # Run unit tests - - make test + - ctest -j4 --output-on-failure # Make sure we can install and uninstall with no issues - sudo make -j4 install - sudo make -j4 uninstall - -after_failure: - - cat Testing/Temporary/LastTest.log - - cat Testing/Temporary/LastTestsFailed.log - diff --git a/CHANGELOG.md b/CHANGELOG.md index 7f2bebd6d..8b1bd3457 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,11 +11,24 @@ * Math * Switched to Eigen for math operations: [#96](https://github.com/flexible-collision-library/fcl/issues/96), [#150](https://github.com/flexible-collision-library/fcl/pull/150) + * fcl::Transform defined to be an Isometry to facilitate inverses [#318](https://github.com/flexible-collision-library/fcl/pull/318) + +* Geometry + + * Simplified Convex class, deprecating old constructor in favor of + simpler, documented constructor: + [#325](https://github.com/flexible-collision-library/fcl/pull/325), + [#338](https://github.com/flexible-collision-library/fcl/pull/338) * Broadphase * Fixed redundant pair checking of SpatialHashingCollisionManager: [#156](https://github.com/flexible-collision-library/fcl/pull/156) +* Narrowphase + + * Add custom sphere-box collision and distance algorithms for both solvers: [#316](https://github.com/flexible-collision-library/fcl/pull/316) + * Add custom sphere-cylinder collision and distance algorithms for both solvers: [#321](https://github.com/flexible-collision-library/fcl/pull/321) + * Distance * Added distance request option for computing exact negative distance: [#172](https://github.com/flexible-collision-library/fcl/pull/172) @@ -26,6 +39,7 @@ * Added CMake targets for generating API documentation: [#174](https://github.com/flexible-collision-library/fcl/pull/174) * Enabled build with SSE option by default: [#159](https://github.com/flexible-collision-library/fcl/pull/159) * Added missing copyright headers: [#149](https://github.com/flexible-collision-library/fcl/pull/149) + * Added test utility for performing equality between Eigen matrix-types (`CompareMatrices` in `test/eign_matrix_compare.h`): [#316](https://github.com/flexible-collision-library/fcl/pull/316) ### FCL 0.5.0 (2016-07-19) diff --git a/CMakeLists.txt b/CMakeLists.txt index 95df30830..8b05c4dac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,26 +1,54 @@ +# -*- mode: cmake -*- +# vi: set ft=cmake : + +# Copyright (c) 2012, Willow Garage, Inc. +# Copyright (c) 2016, Toyota Research Institute, Inc. +# 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 HOLDER 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. + if(APPLE) - cmake_minimum_required(VERSION 3.0.00) + cmake_minimum_required(VERSION 3.0.0) elseif(MSVC) cmake_minimum_required(VERSION 3.1.3) else() cmake_minimum_required(VERSION 2.8.12) endif() -# Set the CMAKE_CXX_COMPILER_ID variable to AppleClang instead of Clang. -# AppleClang and Clang have different version number. This was introduced in -# CMake 3.0. +project(fcl CXX) + +# Compiler ID for Apple Clang is now AppleClang. if(POLICY CMP0025) cmake_policy(SET CMP0025 NEW) endif() -# Use MACOSX_RPATH by default on OS X. This was added in CMake 2.8.12 and -# became default in CMake 3.0. Explicitly setting this policy is necessary to -# suppress a warning in CMake 3.0 and above. -if(POLICY CMP0042) - cmake_policy(SET CMP0042 NEW) -endif() +include(CTest) -project(fcl CXX C) +configure_file(CTestCustom.cmake.in CTestCustom.cmake @ONLY) option(FCL_ENABLE_PROFILING "Enable profiling" OFF) option(FCL_TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" OFF) @@ -29,8 +57,12 @@ option(FCL_TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" OFF) option(FCL_HIDE_ALL_SYMBOLS "Hide all binary symbols" OFF) # set the default build type -if (NOT MSVC AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + set(CMAKE_BUILD_TYPE Release CACHE STRING + "Choose the type of build; options are Debug Release RelWithDebInfo MinSizeRel." + FORCE) + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY + STRINGS Debug Release RelWithDebInfo MinSizeRel) endif() # This shouldn't be necessary, but there has been trouble @@ -41,11 +73,11 @@ endif (MSVC OR MSVC90 OR MSVC10) set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib) set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules") +include(GenerateExportHeader) +include(GNUInstallDirs) include(FCLMacros) include(CompilerSettings) include(FCLVersion) -include(GNUInstallDirs) -include(GenerateExportHeader) if(MSVC OR IS_ICPC) option(FCL_STATIC_LIBRARY "Whether the FCL library should be static rather than shared" ON) @@ -272,9 +304,20 @@ configure_file( add_custom_target(uninstall "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/CMakeModules/cmake_uninstall.cmake") -option(FCL_BUILD_TESTS "Build FCL tests" ON) -if(FCL_BUILD_TESTS AND NOT FCL_HIDE_ALL_SYMBOLS) - enable_testing() +set(FCL_BUILD_TESTS "DEFAULT" CACHE INTERNAL "Deprecated; use BUILD_TESTING instead.") + +if(NOT FCL_BUILD_TESTS STREQUAL "DEFAULT") + message(DEPRECATION "FCL_BUILD_TESTS is deprecated; use BUILD_TESTING instead.") + if(FCL_BUILD_TESTS) + set(_BUILD_TESTING ON) + else() + set(_BUILD_TESTING OFF) + endif() + set(BUILD_TESTING ${_BUILD_TESTING} CACHE BOOL "Build the testing tree." FORCE) + unset(_BUILD_TESTING) +endif() + +if(BUILD_TESTING AND NOT FCL_HIDE_ALL_SYMBOLS) add_subdirectory(test) endif() diff --git a/CMakeModules/CompilerSettings.cmake b/CMakeModules/CompilerSettings.cmake index f32e77395..96b9e1f73 100644 --- a/CMakeModules/CompilerSettings.cmake +++ b/CMakeModules/CompilerSettings.cmake @@ -1,3 +1,36 @@ +# -*- mode: cmake -*- +# vi: set ft=cmake : + +# Copyright (c) 2013, Willow Garage, Inc. +# Copyright (c) 2017, Toyota Research Institute, Inc. +# 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 HOLDER 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. + # GCC if(CMAKE_COMPILER_IS_GNUCXX) add_definitions(-std=c++11 -W -Wall -Wextra -Wpedantic) @@ -85,12 +118,9 @@ mark_as_advanced(FCL_NO_DEFAULT_RPATH) # Set rpath http://www.paraview.org/Wiki/CMake_RPATH_handling if(FCL_NO_DEFAULT_RPATH) - message(STATUS "Set RPATH explicitly to '${CMAKE_INSTALL_LIBDIR_FULL}'") - set(CMAKE_SKIP_BUILD_RPATH FALSE) - set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) - set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_LIBDIR_FULL}") - set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + message(STATUS "Set RPATH explicitly to '${CMAKE_INSTALL_FULL_LIBDIR}'") + set(CMAKE_SKIP_BUILD_RPATH OFF) + set(CMAKE_BUILD_WITH_INSTALL_RPATH OFF) + set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_FULL_LIBDIR}") + set(CMAKE_INSTALL_RPATH_USE_LINK_PATH ON) endif() - -# no prefix needed for python modules -set(CMAKE_SHARED_MODULE_PREFIX "") diff --git a/CPPLINT.cfg b/CPPLINT.cfg new file mode 100644 index 000000000..e9c47a603 --- /dev/null +++ b/CPPLINT.cfg @@ -0,0 +1,36 @@ +# Copyright (c) 2018, Toyota Research Institute, Inc. +# 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 HOLDER 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: Jamie Snape, Kitware, Inc. + +set noparent + +extensions=cc,cpp,h +filter=-build/c++11,-build/c++14 +root=include diff --git a/CTestConfig.cmake b/CTestConfig.cmake new file mode 100644 index 000000000..6871b27ab --- /dev/null +++ b/CTestConfig.cmake @@ -0,0 +1,40 @@ +# -*- mode: cmake -*- +# vi: set ft=cmake : + +# Copyright (c) 2018, Toyota Research Institute, Inc. +# 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 HOLDER 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: Jamie Snape, Kitware, Inc. + +set(CTEST_PROJECT_NAME fcl) +set(CTEST_NIGHTLY_START_TIME "00:00:00 EST") +set(CTEST_DROP_METHOD https) +set(CTEST_DROP_SITE drake-cdash.csail.mit.edu) +set(CTEST_DROP_LOCATION "/submit.php?project=${CTEST_PROJECT_NAME}") +set(CTEST_DROP_SITE_CDASH ON) diff --git a/CTestCustom.cmake.in b/CTestCustom.cmake.in new file mode 100644 index 000000000..dffbdfa62 --- /dev/null +++ b/CTestCustom.cmake.in @@ -0,0 +1,38 @@ +# -*- mode: cmake -*- +# vi: set ft=cmake : + +# Copyright (c) 2018, Toyota Research Institute, Inc. +# 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 HOLDER 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: Jamie Snape, Kitware, Inc. + +list(APPEND CTEST_CUSTOM_COVERAGE_EXCLUDE ".*/test/.*") + +set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_ERRORS 128) +set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_WARNINGS 128) diff --git a/README.md b/README.md index b81b622f3..8d9e459c1 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ ## FCL -- The Flexible Collision Library Linux / OS X [![Build Status](https://travis-ci.org/flexible-collision-library/fcl.svg?branch=master)](https://travis-ci.org/flexible-collision-library/fcl) -Windows [![Build status](https://ci.appveyor.com/api/projects/status/rrmxadnj1empitqq/branch/master?svg=true)](https://ci.appveyor.com/project/mamoll/fcl) +Windows [![Build status](https://ci.appveyor.com/api/projects/status/do1k727uu6e8uemf/branch/master?svg=true)](https://ci.appveyor.com/project/flexible-collision-library/fcl) Coverage [![Coverage Status](https://coveralls.io/repos/github/flexible-collision-library/fcl/badge.svg?branch=master)](https://coveralls.io/github/flexible-collision-library/fcl?branch=master) FCL is a library for performing three types of proximity queries on a pair of geometric models composed of triangles. diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in index a8090d6f7..230daa70e 100644 --- a/doc/Doxyfile.in +++ b/doc/Doxyfile.in @@ -2037,7 +2037,7 @@ INCLUDE_FILE_PATTERNS = # recursively expanded use the := operator instead of the = operator. # This tag requires that the tag ENABLE_PREPROCESSING is set to YES. -PREDEFINED = +PREDEFINED = FCL_DOXYGEN_CXX=1 # If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this # tag can be used to specify a list of macro names that should be expanded. The diff --git a/include/fcl/common/types.h b/include/fcl/common/types.h index b5187fa62..53f49d711 100644 --- a/include/fcl/common/types.h +++ b/include/fcl/common/types.h @@ -86,7 +86,7 @@ template using Quaternion = Eigen::Quaternion; template -using Transform3 = Eigen::Transform; +using Transform3 = Eigen::Transform; template using Translation3 = Eigen::Translation; diff --git a/include/fcl/geometry/collision_geometry-inl.h b/include/fcl/geometry/collision_geometry-inl.h index 0fbfc01f6..b5695325e 100644 --- a/include/fcl/geometry/collision_geometry-inl.h +++ b/include/fcl/geometry/collision_geometry-inl.h @@ -141,6 +141,13 @@ S CollisionGeometry::computeVolume() const template Matrix3 CollisionGeometry::computeMomentofInertiaRelatedToCOM() const { + // TODO(SeanCurtis-TRI): This is *horribly* inefficient. In complex cases, + // this will require computing volume integrals three times. The + // CollisionGeometry class should have a single virtual function that will + // return all three quantities in one call so that particular sub-classes can + // override this to process this answer more efficiently. The default + // implementation can be exactly these three calls. + // See: https://github.com/flexible-collision-library/fcl/issues/327. Matrix3 C = computeMomentofInertia(); Vector3 com = computeCOM(); S V = computeVolume(); diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h index bafdfea73..7fe43860c 100644 --- a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h +++ b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h @@ -44,8 +44,29 @@ namespace fcl { //============================================================================== +// Local helper function to ease conditional adding of triangles to a BVHModel template -void generateBVHModel(BVHModel& model, const Box& shape, const Transform3& pose) +int addTriangles(BVHModel& model, const std::vector>& points, const std::vector& tri_indices, FinalizeModel finalize_model) +{ + int retval = BVH_OK; + if(model.build_state == BVH_BUILD_STATE_EMPTY){ + retval = model.beginModel(); + } + + if(retval == BVH_OK){ + retval = model.addSubModel(points, tri_indices); + } + + if(retval == BVH_OK && finalize_model == FinalizeModel::DO){ + retval = model.endModel(); + model.computeLocalAABB(); + } + return retval; +} + +//============================================================================== +template +int generateBVHModel(BVHModel& model, const Box& shape, const Transform3& pose, FinalizeModel finalize_model) { using S = typename BV::S; @@ -81,15 +102,13 @@ void generateBVHModel(BVHModel& model, const Box& shape, con points[i] = pose * points[i]; } - model.beginModel(); - model.addSubModel(points, tri_indices); - model.endModel(); - model.computeLocalAABB(); + return addTriangles(model, points, tri_indices, finalize_model); } + //============================================================================== template -void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int seg, unsigned int ring) +int generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model) { using S = typename BV::S; @@ -148,15 +167,12 @@ void generateBVHModel(BVHModel& model, const Sphere& shape, points[i] = pose * points[i]; } - model.beginModel(); - model.addSubModel(points, tri_indices); - model.endModel(); - model.computeLocalAABB(); + return addTriangles(model, points, tri_indices, finalize_model); } //============================================================================== template -void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int n_faces_for_unit_sphere) +int generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int n_faces_for_unit_sphere, FinalizeModel finalize_model) { using S = typename BV::S; @@ -165,12 +181,12 @@ void generateBVHModel(BVHModel& model, const Sphere& shape, unsigned int ring = ceil(n_low_bound); unsigned int seg = ceil(n_low_bound); - generateBVHModel(model, shape, pose, seg, ring); + return generateBVHModel(model, shape, pose, seg, ring, finalize_model); } //============================================================================== template -void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int seg, unsigned int ring) +int generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model) { using S = typename BV::S; @@ -232,15 +248,12 @@ void generateBVHModel(BVHModel& model, const Ellipsoid& shap points[i] = pose * points[i]; } - model.beginModel(); - model.addSubModel(points, tri_indices); - model.endModel(); - model.computeLocalAABB(); + return addTriangles(model, points, tri_indices, finalize_model); } //============================================================================== template -void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int n_faces_for_unit_ellipsoid) +int generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int n_faces_for_unit_ellipsoid, FinalizeModel finalize_model) { using S = typename BV::S; @@ -256,12 +269,12 @@ void generateBVHModel(BVHModel& model, const Ellipsoid& shap const unsigned int ring = std::ceil(n_low_bound); const unsigned int seg = std::ceil(n_low_bound); - generateBVHModel(model, shape, pose, seg, ring); + return generateBVHModel(model, shape, pose, seg, ring, finalize_model); } //============================================================================== template -void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int tot, unsigned int h_num) +int generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model) { using S = typename BV::S; @@ -272,45 +285,45 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape S h = shape.lz; S phi, phid; const S pi = constants::pi(); - phid = pi * 2 / tot; + phid = pi * 2 / circle_split_tot; phi = 0; S hd = h / h_num; - for(unsigned int i = 0; i < tot; ++i) + for(unsigned int i = 0; i < circle_split_tot; ++i) points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2); for(unsigned int i = 0; i < h_num - 1; ++i) { - for(unsigned int j = 0; j < tot; ++j) + for(unsigned int j = 0; j < circle_split_tot; ++j) { points.emplace_back(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd); } } - for(unsigned int i = 0; i < tot; ++i) + for(unsigned int i = 0; i < circle_split_tot; ++i) points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2); points.emplace_back(0, 0, h / 2); points.emplace_back(0, 0, -h / 2); - for(unsigned int i = 0; i < tot; ++i) - tri_indices.emplace_back((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1))); + for(unsigned int i = 0; i < circle_split_tot; ++i) + tri_indices.emplace_back((h_num + 1) * circle_split_tot, i, ((i == circle_split_tot - 1) ? 0 : (i + 1))); - for(unsigned int i = 0; i < tot; ++i) - tri_indices.emplace_back((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i); + for(unsigned int i = 0; i < circle_split_tot; ++i) + tri_indices.emplace_back((h_num + 1) * circle_split_tot + 1, h_num * circle_split_tot + ((i == circle_split_tot - 1) ? 0 : (i + 1)), h_num * circle_split_tot + i); for(unsigned int i = 0; i < h_num; ++i) { - for(unsigned int j = 0; j < tot; ++j) + for(unsigned int j = 0; j < circle_split_tot; ++j) { 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); + b = (j == circle_split_tot - 1) ? 0 : (j + 1); + c = j + circle_split_tot; + d = (j == circle_split_tot - 1) ? circle_split_tot : (j + 1 + circle_split_tot); - int start = i * tot; + int start = i * circle_split_tot; tri_indices.emplace_back(start + b, start + a, start + c); tri_indices.emplace_back(start + b, start + c, start + d); } @@ -321,15 +334,12 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape points[i] = pose * points[i]; } - model.beginModel(); - model.addSubModel(points, tri_indices); - model.endModel(); - model.computeLocalAABB(); + return addTriangles(model, points, tri_indices, finalize_model); } //============================================================================== template -void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int tot_for_unit_cylinder) +int generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model) { using S = typename BV::S; @@ -337,19 +347,18 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape S h = shape.lz; const S pi = constants::pi(); - unsigned int tot = tot_for_unit_cylinder * r; - S phid = pi * 2 / tot; + unsigned int circle_split_tot = circle_split_tot_for_unit_cylinder * r; + S phid = pi * 2 / circle_split_tot; S circle_edge = phid * r; unsigned int h_num = ceil(h / circle_edge); - generateBVHModel(model, shape, pose, tot, h_num); + return generateBVHModel(model, shape, pose, circle_split_tot, h_num, finalize_model); } - //============================================================================== template -void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int tot, unsigned int h_num) +int generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model) { using S = typename BV::S; @@ -361,7 +370,7 @@ void generateBVHModel(BVHModel& model, const Cone& shape, co S phi, phid; const S pi = constants::pi(); - phid = pi * 2 / tot; + phid = pi * 2 / circle_split_tot; phi = 0; S hd = h / h_num; @@ -370,35 +379,35 @@ void generateBVHModel(BVHModel& model, const Cone& shape, co { S h_i = h / 2 - (i + 1) * hd; S rh = r * (0.5 - h_i / h); - for(unsigned int j = 0; j < tot; ++j) + for(unsigned int j = 0; j < circle_split_tot; ++j) { points.emplace_back(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i); } } - for(unsigned int i = 0; i < tot; ++i) + for(unsigned int i = 0; i < circle_split_tot; ++i) points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2); points.emplace_back(0, 0, h / 2); points.emplace_back(0, 0, -h / 2); - for(unsigned int i = 0; i < tot; ++i) - tri_indices.emplace_back(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1)); + for(unsigned int i = 0; i < circle_split_tot; ++i) + tri_indices.emplace_back(h_num * circle_split_tot, i, (i == circle_split_tot - 1) ? 0 : (i + 1)); - for(unsigned int i = 0; i < tot; ++i) - tri_indices.emplace_back(h_num * tot + 1, (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), (h_num - 1) * tot + i); + for(unsigned int i = 0; i < circle_split_tot; ++i) + tri_indices.emplace_back(h_num * circle_split_tot + 1, (h_num - 1) * circle_split_tot + ((i == circle_split_tot - 1) ? 0 : (i + 1)), (h_num - 1) * circle_split_tot + i); for(unsigned int i = 0; i < h_num - 1; ++i) { - for(unsigned int j = 0; j < tot; ++j) + for(unsigned int j = 0; j < circle_split_tot; ++j) { 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); + b = (j == circle_split_tot - 1) ? 0 : (j + 1); + c = j + circle_split_tot; + d = (j == circle_split_tot - 1) ? circle_split_tot : (j + 1 + circle_split_tot); - int start = i * tot; + int start = i * circle_split_tot; tri_indices.emplace_back(start + b, start + a, start + c); tri_indices.emplace_back(start + b, start + c, start + d); } @@ -409,31 +418,30 @@ void generateBVHModel(BVHModel& model, const Cone& shape, co points[i] = pose * points[i]; } - model.beginModel(); - model.addSubModel(points, tri_indices); - model.endModel(); - model.computeLocalAABB(); + return addTriangles(model, points, tri_indices, finalize_model); + } //============================================================================== template -void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int tot_for_unit_cone) +int generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int circle_split_tot_for_unit_cone, FinalizeModel finalize_model) { using S = typename BV::S; + S r = shape.radius; S h = shape.lz; const S pi = constants::pi(); - unsigned int tot = tot_for_unit_cone * r; - S phid = pi * 2 / tot; + unsigned int circle_split_tot = circle_split_tot_for_unit_cone * r; + S phid = pi * 2 / circle_split_tot; S circle_edge = phid * r; unsigned int h_num = ceil(h / circle_edge); - generateBVHModel(model, shape, pose, tot, h_num); + return generateBVHModel(model, shape, pose, circle_split_tot, h_num, finalize_model); } } // namespace fcl -#endif +#endif \ No newline at end of file diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model.h b/include/fcl/geometry/geometric_shape_to_BVH_model.h index c18825d07..d44350d38 100644 --- a/include/fcl/geometry/geometric_shape_to_BVH_model.h +++ b/include/fcl/geometry/geometric_shape_to_BVH_model.h @@ -52,54 +52,113 @@ namespace fcl { +/** +@brief enum class used to indicate whether we simply want to add more primitives to the model + or finalize it. +*/ +enum class FinalizeModel{ + DO, + DONT +}; + + +/** +@defgroup generateBVHModel +@brief Create a BVHModel using geometric primitives +@details The functions in this group can be used to add geometric primitives (Box, Sphere, Ellipsoid, Cylinder, Cone) + to a BVHModel. It can either close off the model or leave it unfinalized in order to add more primitives later. +@note All functions in this group have a common sub-set of parameters (listed below). In addition, each has unique + parameters related to the geometry type being added and how it should be tessellated. These additional parameters + are documented with their corresponding function +@warning If this function is used to create a BVHModel containing multiple geometric primitives, the BVHModel inherently + represents the *union* of those primitives. The BVHModel structure does not retain any notion of the original + geometric primitive. +@param[out] model The BVHModel to be generated or added to +@param[in] shape The geometric object to be added to the BVHModel +@param[in] pose The pose of the geometric object +@param[in] finalize_model an enum indicating whether the model is final or more submodels can be added later +@return Return code (as defined by BVHReturnCode) indicating the success of the operation +@{ +*/ /// @brief Generate BVH model from box template -void generateBVHModel(BVHModel& model, const Box& shape, const Transform3& pose); +int generateBVHModel(BVHModel& model, const Box& shape, const Transform3& pose, FinalizeModel finalize_model = FinalizeModel::DO); -/// @brief Generate BVH model from sphere, given the number of segments along longitude and number of rings along latitude. +/** +@brief Generate BVH model from sphere +@param[in] seg The number of segments along longitude +@param[in] ring The number of rings along latitude +**/ template -void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int seg, unsigned int ring); - -/// @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 +int generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model = FinalizeModel::DO); + +/** +@brief Generate BVH model from sphere +@details 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. +@param[in] n_faces_for_unit_sphere The number of triangles for a unit-sized sphere +**/ template -void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int n_faces_for_unit_sphere); +int generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int n_faces_for_unit_sphere, FinalizeModel finalize_model = FinalizeModel::DO); -/// @brief Generate BVH model from ellipsoid, given the number of segments along longitude and number of rings along latitude. +/** +@brief Generate BVH model from ellipsoid +@param[in] seg The number of segments along longitude +@param[in] ring The number of rings along latitude +**/ template -void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int seg, unsigned int ring); - -/// @brief Generate BVH model from ellipsoid -/// The difference between generateBVHModel is that it gives the number of triangles faces N for an ellipsoid with unit radii (1, 1, 1). For ellipsoid of radii (a, b, c), -/// then the number of triangles is ((a^p * b^p + b^p * c^p + c^p * a^p)/3)^(1/p) * N, where p is 1.6075, so that the area represented by a single triangle is approximately the same. -/// Reference: https://en.wikipedia.org/wiki/Ellipsoid#Approximate_formula +int generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model = FinalizeModel::DO); + +/** +@brief Generate BVH model from ellipsoid +@details The difference between generateBVHModel is that it gives the number of triangles faces N for an ellipsoid with unit radii (1, 1, 1). For ellipsoid of radii (a, b, c), + then the number of triangles is ((a^p * b^p + b^p * c^p + c^p * a^p)/3)^(1/p) * N, where p is 1.6075, so that the area represented by a single triangle is approximately the same. + Reference: https://en.wikipedia.org/wiki/Ellipsoid#Approximate_formula +@param[in] n_faces_for_unit_ellipsoid The number of faces a unit ellipsoid would have +**/ template -void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int n_faces_for_unit_ellipsoid); +int generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int n_faces_for_unit_ellipsoid, FinalizeModel finalize_model = FinalizeModel::DO); -/// @brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis. +/** +@brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis. +@param[in] circle_split_tot The number of segments the bottom plate of the cylinder is split into +@param[in] h_num The number of segments along the axis the cylinder is split into +**/ template -void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int tot, unsigned int h_num); - -/// @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. +int generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO); + +/** +@brief Generate BVH model from cylinder +@details 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. +@param[in] circle_split_tot_for_unit_cylinder The number of segments the bottom plate of an equivalent unit-sized cylinder would be split into +**/ template -void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int tot_for_unit_cylinder); +int generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model = FinalizeModel::DO); -/// @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 Transform3& pose, unsigned int tot, unsigned int h_num); -/// @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. +/** +@brief Generate BVH model from cone +@param[in] circle_split_tot The number of segments the bottom plate is split into +@param[in] h_num an The number of segments along the axis the cone is split into +**/ +template +int generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO); + +/** +@brief Generate BVH model from cone +@details 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. +@param[in] circle_split_tot_for_unit_cone The number of segments the bottom plate of an equivalent unit-sized cone would be split into +**/ template -void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int tot_for_unit_cone); +int generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int circle_split_tot_for_unit_cone, FinalizeModel finalize_model = FinalizeModel::DO); + +/**@} */ // end of doxygen group generateBVHModel } // namespace fcl #include "fcl/geometry/geometric_shape_to_BVH_model-inl.h" -#endif +#endif \ No newline at end of file diff --git a/include/fcl/geometry/shape/box.h b/include/fcl/geometry/shape/box.h index 1ec03d90f..d15e9f269 100644 --- a/include/fcl/geometry/shape/box.h +++ b/include/fcl/geometry/shape/box.h @@ -40,6 +40,8 @@ #include "fcl/geometry/shape/shape_base.h" +#include + namespace fcl { @@ -78,6 +80,12 @@ class FCL_EXPORT Box : public ShapeBase /// @brief get the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& tf) const; + + friend + std::ostream& operator<<(std::ostream& out, const Box& box) { + out << "Box" << box.side.transpose(); + return out; + } }; using Boxf = Box; diff --git a/include/fcl/geometry/shape/capsule.h b/include/fcl/geometry/shape/capsule.h index 8bb3ffbea..5add95e27 100644 --- a/include/fcl/geometry/shape/capsule.h +++ b/include/fcl/geometry/shape/capsule.h @@ -38,6 +38,8 @@ #ifndef FCL_SHAPE_CAPSULE_H #define FCL_SHAPE_CAPSULE_H +#include + #include "fcl/geometry/shape/shape_base.h" namespace fcl @@ -75,6 +77,12 @@ class FCL_EXPORT Capsule : public ShapeBase /// @brief get the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& tf) const; + + friend + std::ostream& operator<<(std::ostream& out, const Capsule& capsule) { + out << "Capsule(r: " << capsule.radius << ", lz: " << capsule.lz << ")"; + return out; + } }; using Capsulef = Capsule; diff --git a/include/fcl/geometry/shape/cone.h b/include/fcl/geometry/shape/cone.h index f4cc617fe..ef9180c2a 100644 --- a/include/fcl/geometry/shape/cone.h +++ b/include/fcl/geometry/shape/cone.h @@ -38,6 +38,8 @@ #ifndef FCL_SHAPE_CONE_H #define FCL_SHAPE_CONE_H +#include + #include "fcl/geometry/shape/shape_base.h" namespace fcl @@ -77,6 +79,12 @@ class FCL_EXPORT Cone : public ShapeBase /// @brief get the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& tf) const; + + friend + std::ostream& operator<<(std::ostream& out, const Cone& cone) { + out << "Cone(r: " << cone.radius << ", lz: " << cone.lz << ")"; + return out; + } }; using Conef = Cone; diff --git a/include/fcl/geometry/shape/convex-inl.h b/include/fcl/geometry/shape/convex-inl.h index eefe94a69..597545e44 100644 --- a/include/fcl/geometry/shape/convex-inl.h +++ b/include/fcl/geometry/shape/convex-inl.h @@ -34,6 +34,7 @@ */ /** @author Jia Pan */ +/** @author Sean Curtis (2018) Modify API and correct implementation bugs. */ #ifndef FCL_SHAPE_CONVEX_INL_H #define FCL_SHAPE_CONVEX_INL_H @@ -50,58 +51,32 @@ class FCL_EXPORT Convex; //============================================================================== template Convex::Convex( - Vector3* plane_normals, S* plane_dis, int num_planes_, - Vector3* points, int num_points_, int* polygons_) - : ShapeBase() -{ - plane_normals = plane_normals; - plane_dis = plane_dis; - num_planes = num_planes_; - points = points; - num_points = num_points_; - polygons = polygons_; - edges = nullptr; - + const std::shared_ptr>>& vertices, + int num_faces, const std::shared_ptr>& faces) + : ShapeBase(), + vertices_(vertices), + num_faces_(num_faces), + faces_(faces) { + assert(vertices != nullptr); + assert(faces != nullptr); + // Compute an interior point. We're computing the mean point and *not* some + // alternative such as the centroid or bounding box center. Vector3 sum = Vector3::Zero(); - for(int i = 0; i < num_points; ++i) - { - sum += points[i]; + for (const auto& vertex : *vertices_) { + sum += vertex; } - - center = sum * (S)(1.0 / num_points); - - fillEdges(); -} - -//============================================================================== -template -Convex::Convex(const Convex& other) - : ShapeBase(other) -{ - plane_normals = other.plane_normals; - plane_dis = other.plane_dis; - num_planes = other.num_planes; - points = other.points; - polygons = other.polygons; - edges = new Edge[other.num_edges]; - memcpy(edges, other.edges, sizeof(Edge) * num_edges); + interior_point_ = sum * (S)(1.0 / vertices_->size()); } //============================================================================== template -Convex::~Convex() -{ - delete [] edges; -} +void Convex::computeLocalAABB() { + this->aabb_local.min_.setConstant(std::numeric_limits::max()); + this->aabb_local.max_.setConstant(-std::numeric_limits::max()); -//============================================================================== -template -void Convex::computeLocalAABB() -{ - this->aabb_local.min_.setConstant(-std::numeric_limits::max()); - this->aabb_local.max_.setConstant(std::numeric_limits::max()); - for(int i = 0; i < num_points; ++i) - this->aabb_local += points[i]; + for (const auto& v : *vertices_) { + this->aabb_local += v; + } this->aabb_center = this->aabb_local.center(); this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm(); @@ -109,15 +84,20 @@ void Convex::computeLocalAABB() //============================================================================== template -NODE_TYPE Convex::getNodeType() const -{ +NODE_TYPE Convex::getNodeType() const { return GEOM_CONVEX; } //============================================================================== +// TODO(SeanCurtis-TRI): When revisiting these, consider the following +// resources: +// https://www.geometrictools.com/Documentation/PolyhedralMassProperties.pdf +// http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.56.127&rep=rep1&type=pdf +// http://number-none.com/blow/inertia/bb_inertia.doc template -Matrix3 Convex::computeMomentofInertia() const -{ +Matrix3 Convex::computeMomentofInertia() const { + const std::vector>& vertices = *vertices_; + const std::vector& faces = *faces_; Matrix3 C = Matrix3::Zero(); Matrix3 C_canonical; @@ -125,35 +105,37 @@ Matrix3 Convex::computeMomentofInertia() const 1/120.0, 1/ 60.0, 1/120.0, 1/120.0, 1/120.0, 1/ 60.0; - int* points_in_poly = polygons; - int* index = polygons + 1; - for(int i = 0; i < num_planes; ++i) - { - Vector3 plane_center = Vector3::Zero(); - - // compute the center of the polygon - for(int j = 0; j < *points_in_poly; ++j) - plane_center += points[index[j]]; - plane_center = plane_center * (1.0 / *points_in_poly); - - // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape - const Vector3& v3 = plane_center; - for(int j = 0; j < *points_in_poly; ++j) - { - int e_first = index[j]; - int e_second = index[(j+1)%*points_in_poly]; - const Vector3& v1 = points[e_first]; - const Vector3& v2 = points[e_second]; + S vol_times_six = 0; + int face_index = 0; + for (int i = 0; i < num_faces_; ++i) { + const int vertex_count = faces[face_index]; + Vector3 face_center = Vector3::Zero(); + + // Compute the center of the face. + for (int j = 1; j <= vertex_count; ++j) { + face_center += vertices[faces[face_index + j]]; + } + face_center = face_center * (1.0 / vertex_count); + + // Compute the volume of tetrahedron formed by the vertices on one of the + // polygon's edges, the center point, and the shape's frame's origin. + const Vector3& v3 = face_center; + const int vertex_base = face_index + 1; + for (int j = 0; j < vertex_count; ++j) { + int e_first = faces[vertex_base + j]; + int e_second = faces[vertex_base + (j + 1) % vertex_count]; + const Vector3& v1 = vertices[e_first]; + const Vector3& v2 = vertices[e_second]; S d_six_vol = (v1.cross(v2)).dot(v3); Matrix3 A; // this is A' in the original document A.row(0) = v1; A.row(1) = v2; A.row(2) = v3; C += A.transpose() * C_canonical * A * d_six_vol; // change accordingly + vol_times_six += d_six_vol; } - points_in_poly += (*points_in_poly + 1); - index = points_in_poly + 1; + face_index += vertex_count + 1; } S trace_C = C(0, 0) + C(1, 1) + C(2, 2); @@ -163,148 +145,104 @@ Matrix3 Convex::computeMomentofInertia() const -C(1, 0), trace_C - C(1, 1), -C(1, 2), -C(2, 0), -C(2, 1), trace_C - C(2, 2); - return m; + return m * (6 / vol_times_six); } //============================================================================== template -Vector3 Convex::computeCOM() const -{ +Vector3 Convex::computeCOM() const { + const std::vector>& vertices = *vertices_; + const std::vector& faces = *faces_; Vector3 com = Vector3::Zero(); S vol = 0; - int* points_in_poly = polygons; - int* index = polygons + 1; - for(int i = 0; i < num_planes; ++i) - { - Vector3 plane_center = Vector3::Zero(); - - // compute the center of the polygon - for(int j = 0; j < *points_in_poly; ++j) - plane_center += points[index[j]]; - plane_center = plane_center * (1.0 / *points_in_poly); - - // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape - const Vector3& v3 = plane_center; - for(int j = 0; j < *points_in_poly; ++j) - { - int e_first = index[j]; - int e_second = index[(j+1)%*points_in_poly]; - const Vector3& v1 = points[e_first]; - const Vector3& v2 = points[e_second]; + int face_index = 0; + for (int i = 0; i < num_faces_; ++i) { + const int vertex_count = faces[face_index]; + Vector3 face_center = Vector3::Zero(); + + // TODO(SeanCurtis-TRI): See note in computeVolume() on the efficiency of + // this approach. + + // Compute the center of the polygon. + for (int j = 1; j <= vertex_count; ++j) { + face_center += vertices[faces[face_index + j]]; + } + face_center = face_center * (1.0 / vertex_count); + + // Compute the volume of tetrahedron formed by the vertices on one of the + // polygon's edges, the center point, and the shape's frame's origin. + const Vector3& v3 = face_center; + for (int j = 1; j <= vertex_count; ++j) { + int e_first = faces[face_index + j]; + int e_second = faces[face_index + (j % vertex_count) + 1]; + const Vector3& v1 = vertices[e_first]; + const Vector3& v2 = vertices[e_second]; S d_six_vol = (v1.cross(v2)).dot(v3); vol += d_six_vol; - com += (points[e_first] + points[e_second] + plane_center) * d_six_vol; + com += (v1 + v2 + face_center) * d_six_vol; } - points_in_poly += (*points_in_poly + 1); - index = points_in_poly + 1; + face_index += vertex_count + 1; } return com / (vol * 4); // here we choose zero as the reference } //============================================================================== -template -S Convex::computeVolume() const -{ +template S Convex::computeVolume() const { + const std::vector>& vertices = *vertices_; + const std::vector& faces = *faces_; S vol = 0; - int* points_in_poly = polygons; - int* index = polygons + 1; - for(int i = 0; i < num_planes; ++i) - { - Vector3 plane_center = Vector3::Zero(); - - // compute the center of the polygon - for(int j = 0; j < *points_in_poly; ++j) - plane_center += points[index[j]]; - plane_center = plane_center * (1.0 / *points_in_poly); - - // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero point) of the convex shape - const Vector3& v3 = plane_center; - for(int j = 0; j < *points_in_poly; ++j) - { - int e_first = index[j]; - int e_second = index[(j+1)%*points_in_poly]; - const Vector3& v1 = points[e_first]; - const Vector3& v2 = points[e_second]; + int face_index = 0; + for (int i = 0; i < num_faces_; ++i) { + const int vertex_count = faces[face_index]; + Vector3 face_center = Vector3::Zero(); + + // TODO(SeanCurtis-TRI): While this is general, this is inefficient. If the + // face happens to be a triangle, this does 3X the requisite work. + // If the face is a 4-gon, then this does 2X the requisite work. + // As N increases in the N-gon this approach's inherent relative penalty + // shrinks. Ideally, this should at least key on 3-gon and 4-gon before + // falling through to this. + + // Compute the center of the polygon. + for (int j = 1; j <= vertex_count; ++j) { + face_center += vertices[faces[face_index + j]]; + } + face_center = face_center * (1.0 / vertex_count); + + // TODO(SeanCurtis-TRI): Because volume serves as the weights for + // center-of-mass an inertia computations, it should be refactored into its + // own function that can be invoked by providing three vertices (the fourth + // being the origin). + + // Compute the volume of tetrahedron formed by the vertices on one of the + // polygon's edges, the center point, and the shape's frame's origin. + const Vector3& v3 = face_center; + for (int j = 1; j <= vertex_count; ++j) { + int e_first = faces[face_index + j]; + int e_second = faces[face_index + (j % vertex_count) + 1]; + const Vector3& v1 = vertices[e_first]; + const Vector3& v2 = vertices[e_second]; S d_six_vol = (v1.cross(v2)).dot(v3); vol += d_six_vol; } - points_in_poly += (*points_in_poly + 1); - index = points_in_poly + 1; + face_index += vertex_count + 1; } return vol / 6; } -//============================================================================== -template -void Convex::fillEdges() -{ - int* points_in_poly = polygons; - if(edges) delete [] edges; - - int num_edges_alloc = 0; - for(int i = 0; i < num_planes; ++i) - { - num_edges_alloc += *points_in_poly; - points_in_poly += (*points_in_poly + 1); - } - - edges = new Edge[num_edges_alloc]; - - points_in_poly = polygons; - int* index = polygons + 1; - num_edges = 0; - Edge e; - bool isinset; - for(int i = 0; i < num_planes; ++i) - { - for(int j = 0; j < *points_in_poly; ++j) - { - e.first = std::min(index[j], index[(j+1)%*points_in_poly]); - e.second = std::max(index[j], index[(j+1)%*points_in_poly]); - isinset = false; - for(int k = 0; k < num_edges; ++k) - { - if((edges[k].first == e.first) && (edges[k].second == e.second)) - { - isinset = true; - break; - } - } - - if(!isinset) - { - edges[num_edges].first = e.first; - edges[num_edges].second = e.second; - ++num_edges; - } - } - - points_in_poly += (*points_in_poly + 1); - index = points_in_poly + 1; - } - - if(num_edges < num_edges_alloc) - { - Edge* tmp = new Edge[num_edges]; - memcpy(tmp, edges, num_edges * sizeof(Edge)); - delete [] edges; - edges = tmp; - } -} - //============================================================================== template std::vector> Convex::getBoundVertices( - const Transform3& tf) const -{ - std::vector> result(num_points); - for(int i = 0; i < num_points; ++i) - { - result[i] = tf * points[i]; + const Transform3& tf) const { + std::vector> result; + result.reserve(vertices_->size()); + + for (const auto& v : *vertices_) { + result.push_back(tf * v); } return result; diff --git a/include/fcl/geometry/shape/convex.h b/include/fcl/geometry/shape/convex.h index 11d663cc5..03c0489a5 100644 --- a/include/fcl/geometry/shape/convex.h +++ b/include/fcl/geometry/shape/convex.h @@ -34,16 +34,49 @@ */ /** @author Jia Pan */ +/** @author Sean Curtis (2018) Streamline API and document. */ #ifndef FCL_SHAPE_CONVEX_H #define FCL_SHAPE_CONVEX_H +#include + #include "fcl/geometry/shape/shape_base.h" namespace fcl { -/// @brief Convex polytope +/// @brief A convex polytope +/// +/// The %Convex class represents a subset of general meshes: convex meshes. +/// The class represents its underlying mesh quite simply: an ordered list of +/// vertices, `V = [v₀, v₁, ..., vₙ₋₁]`, and an ordered list of faces, +/// `F = [f₀, f₁, ..., fₘ₋₁]`, built on those vertices (via vertex _indices_). +/// +/// A mesh is only compatible with %Convex if it satisfies the following +/// properties: +/// - Each face, fᵢ, must be planar and completely lies on a supporting +/// plane πᵢ. The ordered sets `F = [f₀, f₁, ..., fₙ₋₁]` and +/// `Π = [π₀, π₁, ..., πₙ₋₁]` are defined such that face fᵢ has supporting +/// plane πᵢ. +/// - A face fᵢ is defined by an ordered list of vertex indices (e.g., +/// `fᵢ = [iᵢ₀, iᵢ₁, ..., iᵢₖ₋₁]` (for a face with k vertices and k edges). +/// The _ordering_ of the vertex indices must visit the face's vertices in a +/// _counter-clockwise_ order when viewed from outside the mesh and the +/// vertices must all lie on the face's supporting plane. +/// - Define functions `πᵢ(x)` which report the signed distance from point `x` +/// to plane `πᵢ`. To be convex, it must be true that +/// `π(v) ≤ 0, ∀ π ∈ Π ∧ v ∈ V`. +/// +/// If those requirements are satisfied, for a given convex region, the mesh +/// with the smallest number of faces *may* have non-triangular faces. In fact, +/// they could be polygons with an arbitrary number of edges/vertices. The +/// %Convex class supports compact representation. But it *also* supports +/// representations that decompose a large n-gon into a set of smaller polygons +/// or triangles. In this case, each of the triangles supporting planes would +/// be the same and the requirements listed above would still be satisfied. +/// +/// @tparam S_ The scalar type; must be a valid Eigen scalar. template class FCL_EXPORT Convex : public ShapeBase { @@ -51,67 +84,102 @@ class FCL_EXPORT Convex : public ShapeBase using S = S_; - /// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information - Convex(Vector3* plane_normals, - S* plane_dis, - int num_planes, - Vector3* points, - int num_points, - int* polygons); + // TODO(SeanCurtis-TRI): A huge shopping list of issues with this class are + // enumerated in https://github.com/flexible-collision-library/fcl/issues/326. + + /// @brief Constructor + /// + /// @note: The %Convex geometry assumes that the input data %vertices and + /// %faces do not change through the life of the object. + /// + /// @warning: The %Convex class does *not* validate the input; it trusts that + /// the inputs truly represent a coherent convex polytope. + /// + /// @param vertices The positions of the polytope vertices. + /// @param num_faces The number of faces defined in `faces`. + /// @param faces Encoding of the polytope faces. Must encode + /// `num_faces` number of faces. See member + /// documentation for details on encoding. + Convex(const std::shared_ptr>>& vertices, + int num_faces, const std::shared_ptr>& faces); /// @brief Copy constructor Convex(const Convex& other); - ~Convex(); + ~Convex() = default; - /// @brief Compute AABB + /// @brief Computes AABB in the geometry's canonical frame. void computeLocalAABB() override; - /// @brief Get node type: a conex polytope + /// @brief Gets node type: a convex polytope. NODE_TYPE getNodeType() const override; - - Vector3* plane_normals; - S* plane_dis; - - /// @brief An array of indices to the points of each polygon, it should be the number of vertices - /// followed by that amount of indices to "points" in counter clockwise order - int* polygons; - - Vector3* points; - int num_points; - int num_edges; - int num_planes; - - struct Edge - { - int first, second; - }; - - Edge* edges; - - /// @brief center of the convex polytope, this is used for collision: center is guaranteed in the internal of the polytope (as it is convex) - Vector3 center; - - /// based on http://number-none.com/blow/inertia/bb_inertia.doc + /// @brief Gets the vertex positions in the geometry's frame G. + const std::vector>& getVertices() const { return *vertices_; } + + /// @brief Gets the total number of faces in the convex mesh. + int getFaceCount() const { return num_faces_; } + + /// @brief Gets the representation of the *faces* of the convex hull. + /// + /// The array is the concatenation of an integer-based representations of each + /// face. A single face is encoded as a sub-array of ints where the first int + /// is the *number* n of vertices in the face, and the following n values + /// are ordered indices into `vertices` which visit the vertex values in a + /// *counter-clockwise* order (viewed from the outside). + /// + /// For a well-formed face `f` consisting of indices [i₀, i₁, ..., iₘ₋₁], it + /// should be the case that: + /// + /// `eⱼ × eⱼ₊₁ · n̂ₚ = |eⱼ × eⱼ₊₁|, ∀ 0 ≤ j < m, j ∈ ℤ`, where + /// `n̂ₚ` is the normal for plane `π` on which face `f` lies. + /// `eⱼ = vertices[iⱼ] - vertices[iⱼ₋₁]` is the edge vector of face `f` + /// defined by adjacent vertex indices at jᵗʰ vertex (wrapping around such + /// that `j - 1 = m - 1` for `j = 0`). + /// + /// Satisfying this condition implies the following: + /// 1. vertices are not coincident and + /// 3. the indices of the face correspond to a proper counter-clockwise + /// ordering. + const std::vector& getFaces() const { return *faces_; } + + /// @brief A point guaranteed to be on the interior of the convex polytope, + /// used for collision. + const Vector3& getInteriorPoint() const { return interior_point_; } + + // Documentation inherited. Matrix3 computeMomentofInertia() const override; - // Documentation inherited + // Documentation inherited. Vector3 computeCOM() const override; - // Documentation inherited + // Documentation inherited. S computeVolume() const override; - /// @brief get the vertices of some convex shape which can bound this shape in + /// @brief Gets the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& tf) const; -protected: - - /// @brief Get edge information - void fillEdges(); + friend + std::ostream& operator<<(std::ostream& out, const Convex& convex) { + out << "Convex(v count: " << convex.vertices_->size() << ", f count: " + << convex.getFaceCount() << ")"; + return out; + } + +private: + const std::shared_ptr>> vertices_; + const int num_faces_; + const std::shared_ptr> faces_; + Vector3 interior_point_; }; +// Workaround for https://gcc.gnu.org/bugzilla/show_bug.cgi?id=57728 which +// should be moved back into the class definition once we no longer need to +// support GCC versions prior to 6.3. +template +Convex::Convex(const Convex& other) = default; + using Convexf = Convex; using Convexd = Convex; diff --git a/include/fcl/geometry/shape/cylinder.h b/include/fcl/geometry/shape/cylinder.h index 43be1def3..bd67d3132 100644 --- a/include/fcl/geometry/shape/cylinder.h +++ b/include/fcl/geometry/shape/cylinder.h @@ -38,6 +38,8 @@ #ifndef FCL_SHAPE_CYLINDER_H #define FCL_SHAPE_CYLINDER_H +#include + #include "fcl/geometry/shape/shape_base.h" namespace fcl @@ -75,6 +77,12 @@ class FCL_EXPORT Cylinder : public ShapeBase /// @brief get the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& tf) const; + + friend + std::ostream& operator<<(std::ostream& out, const Cylinder& cylinder) { + out << "Cylinder(r: " << cylinder.radius << ", lz: " << cylinder.lz << ")"; + return out; + } }; using Cylinderf = Cylinder; diff --git a/include/fcl/geometry/shape/ellipsoid.h b/include/fcl/geometry/shape/ellipsoid.h index c876be685..85632a900 100644 --- a/include/fcl/geometry/shape/ellipsoid.h +++ b/include/fcl/geometry/shape/ellipsoid.h @@ -38,6 +38,8 @@ #ifndef FCL_SHAPE_ELLIPSOID_H #define FCL_SHAPE_ELLIPSOID_H +#include + #include "fcl/geometry/shape/shape_base.h" namespace fcl @@ -75,6 +77,12 @@ class FCL_EXPORT Ellipsoid : public ShapeBase /// @brief get the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& tf) const; + + friend + std::ostream& operator<<(std::ostream& out, const Ellipsoid& ellipsoid) { + out << "Ellipsoid(radii: " << ellipsoid.radii.transpose() << ")"; + return out; + } }; using Ellipsoidf = Ellipsoid; diff --git a/include/fcl/geometry/shape/halfspace.h b/include/fcl/geometry/shape/halfspace.h index 05f9d0881..374b9e264 100644 --- a/include/fcl/geometry/shape/halfspace.h +++ b/include/fcl/geometry/shape/halfspace.h @@ -38,6 +38,8 @@ #ifndef FCL_SHAPE_HALFSPACE_H #define FCL_SHAPE_HALFSPACE_H +#include + #include "fcl/geometry/shape/shape_base.h" #include "fcl/math/bv/OBB.h" #include "fcl/math/bv/RSS.h" @@ -82,6 +84,13 @@ class FCL_EXPORT Halfspace : public ShapeBase /// @brief Planed offset S d; + friend + std::ostream& operator<<(std::ostream& out, const Halfspace& halfspace) { + out << "Halfspace(n: " << halfspace.n.transpose() << ", d: " + << halfspace.d << ")"; + return out; + } + protected: /// @brief Turn non-unit normal into unit diff --git a/include/fcl/geometry/shape/plane.h b/include/fcl/geometry/shape/plane.h index f3518b1ce..bcf34212e 100644 --- a/include/fcl/geometry/shape/plane.h +++ b/include/fcl/geometry/shape/plane.h @@ -38,6 +38,8 @@ #ifndef FCL_SHAPE_PLANE_H #define FCL_SHAPE_PLANE_H +#include + #include "fcl/geometry/shape/shape_base.h" namespace fcl @@ -75,6 +77,12 @@ class FCL_EXPORT Plane : public ShapeBase /// @brief Plane offset S d; + friend + std::ostream& operator<<(std::ostream& out, const Plane& plane) { + out << "Plane(n: " << plane.n.transpose() << ", d: " << plane.d << ")"; + return out; + } + protected: /// @brief Turn non-unit normal into unit diff --git a/include/fcl/geometry/shape/sphere.h b/include/fcl/geometry/shape/sphere.h index 0c9082573..48b67927f 100644 --- a/include/fcl/geometry/shape/sphere.h +++ b/include/fcl/geometry/shape/sphere.h @@ -40,6 +40,8 @@ #include "fcl/geometry/shape/shape_base.h" +#include + namespace fcl { @@ -69,6 +71,12 @@ class FCL_EXPORT Sphere : public ShapeBase /// @brief get the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& tf) const; + + friend + std::ostream& operator<<(std::ostream& out, const Sphere& sphere) { + out << "Sphere(" << sphere.radius << ")"; + return out; + } }; using Spheref = Sphere; diff --git a/include/fcl/geometry/shape/triangle_p.h b/include/fcl/geometry/shape/triangle_p.h index 7c26fa67a..7cc18caa0 100644 --- a/include/fcl/geometry/shape/triangle_p.h +++ b/include/fcl/geometry/shape/triangle_p.h @@ -38,6 +38,8 @@ #ifndef FCL_SHAPE_TRIANGLE_P_H #define FCL_SHAPE_TRIANGLE_P_H +#include + #include "fcl/geometry/shape/shape_base.h" namespace fcl @@ -68,6 +70,13 @@ class FCL_EXPORT TriangleP : public ShapeBase /// @brief get the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& tf) const; + + friend + std::ostream& operator<<(std::ostream& out, const TriangleP& tri) { + out << "TriangleP(a: " << tri.a.transpose() + << ", b: " << tri.b.transpose() << ", c: " << tri.c.transpose() << ")"; + return out; + } }; using TrianglePf = TriangleP; diff --git a/include/fcl/geometry/shape/utility-inl.h b/include/fcl/geometry/shape/utility-inl.h index 1fa77fc7a..146819daf 100644 --- a/include/fcl/geometry/shape/utility-inl.h +++ b/include/fcl/geometry/shape/utility-inl.h @@ -127,7 +127,8 @@ struct FCL_EXPORT ComputeBVImpl static void run(const Shape& s, const Transform3& tf, BV& bv) { std::vector> convex_bound_vertices = s.getBoundVertices(tf); - fit(&convex_bound_vertices[0], (int)convex_bound_vertices.size(), bv); + fit(convex_bound_vertices.data(), + static_cast(convex_bound_vertices.size()), bv); } }; @@ -234,9 +235,9 @@ struct FCL_EXPORT ComputeBVImpl, Convex> const Vector3& T = tf.translation(); AABB bv_; - for(int i = 0; i < s.num_points; ++i) + for (const auto& vertex : s.getVertices()) { - Vector3 new_p = R * s.points[i] + T; + Vector3 new_p = R * vertex + T; bv_ += new_p; } @@ -250,7 +251,7 @@ struct FCL_EXPORT ComputeBVImpl, Convex> { static void run(const Convex& s, const Transform3& tf, OBB& bv) { - fit(s.points, s.num_points, bv); + fit(s.getVertices().data(), static_cast(s.getVertices().size()), bv); bv.axis = tf.linear(); bv.To = tf * bv.To; diff --git a/include/fcl/math/bv/utility-inl.h b/include/fcl/math/bv/utility-inl.h index a83d37183..eb8653c1b 100644 --- a/include/fcl/math/bv/utility-inl.h +++ b/include/fcl/math/bv/utility-inl.h @@ -63,7 +63,7 @@ namespace OBB_fit_functions { //============================================================================== template FCL_EXPORT -void fit1(Vector3* ps, OBB& bv) +void fit1(const Vector3* const ps, OBB& bv) { bv.To = ps[0]; bv.axis.setIdentity(); @@ -73,7 +73,7 @@ void fit1(Vector3* ps, OBB& bv) //============================================================================== template FCL_EXPORT -void fit2(Vector3* ps, OBB& bv) +void fit2(const Vector3* const ps, OBB& bv) { const Vector3& p1 = ps[0]; const Vector3& p2 = ps[1]; @@ -91,7 +91,7 @@ void fit2(Vector3* ps, OBB& bv) //============================================================================== template FCL_EXPORT -void fit3(Vector3* ps, OBB& bv) +void fit3(const Vector3* const ps, OBB& bv) { const Vector3& p1 = ps[0]; const Vector3& p2 = ps[1]; @@ -121,7 +121,7 @@ void fit3(Vector3* ps, OBB& bv) //============================================================================== template FCL_EXPORT -void fit6(Vector3* ps, OBB& bv) +void fit6(const Vector3* const ps, OBB& bv) { OBB bv1, bv2; fit3(ps, bv1); @@ -132,7 +132,7 @@ void fit6(Vector3* ps, OBB& bv) //============================================================================== template FCL_EXPORT -void fitn(Vector3* ps, int n, OBB& bv) +void fitn(const Vector3* const ps, int n, OBB& bv) { Matrix3 M; Matrix3 E; @@ -148,23 +148,23 @@ void fitn(Vector3* ps, int n, OBB& bv) //============================================================================== extern template -void fit1(Vector3* ps, OBB& bv); +void fit1(const Vector3d* const ps, OBB& bv); //============================================================================== extern template -void fit2(Vector3* ps, OBB& bv); +void fit2(const Vector3d* const ps, OBB& bv); //============================================================================== extern template -void fit3(Vector3* ps, OBB& bv); +void fit3(const Vector3d* const ps, OBB& bv); //============================================================================== extern template -void fit6(Vector3* ps, OBB& bv); +void fit6(const Vector3d* const ps, OBB& bv); //============================================================================== extern template -void fitn(Vector3* ps, int n, OBB& bv); +void fitn(const Vector3d* const ps, int n, OBB& bv); //============================================================================== } // namespace OBB_fit_functions @@ -177,7 +177,7 @@ namespace RSS_fit_functions { //============================================================================== template FCL_EXPORT -void fit1(Vector3* ps, RSS& bv) +void fit1(const Vector3* const ps, RSS& bv) { bv.To = ps[0]; bv.axis.setIdentity(); @@ -189,7 +189,7 @@ void fit1(Vector3* ps, RSS& bv) //============================================================================== template FCL_EXPORT -void fit2(Vector3* ps, RSS& bv) +void fit2(const Vector3* const ps, RSS& bv) { const Vector3& p1 = ps[0]; const Vector3& p2 = ps[1]; @@ -209,7 +209,7 @@ void fit2(Vector3* ps, RSS& bv) //============================================================================== template FCL_EXPORT -void fit3(Vector3* ps, RSS& bv) +void fit3(const Vector3* const ps, RSS& bv) { const Vector3& p1 = ps[0]; const Vector3& p2 = ps[1]; @@ -237,7 +237,7 @@ void fit3(Vector3* ps, RSS& bv) //============================================================================== template FCL_EXPORT -void fit6(Vector3* ps, RSS& bv) +void fit6(const Vector3* const ps, RSS& bv) { RSS bv1, bv2; fit3(ps, bv1); @@ -248,7 +248,7 @@ void fit6(Vector3* ps, RSS& bv) //============================================================================== template FCL_EXPORT -void fitn(Vector3* ps, int n, RSS& bv) +void fitn(const Vector3* const ps, int n, RSS& bv) { Matrix3 M; // row first matrix Matrix3 E; // row first eigen-vectors @@ -265,27 +265,27 @@ void fitn(Vector3* ps, int n, RSS& bv) //============================================================================== extern template FCL_EXPORT -void fit1(Vector3* ps, RSS& bv); +void fit1(const Vector3d* const ps, RSS& bv); //============================================================================== extern template FCL_EXPORT -void fit2(Vector3* ps, RSS& bv); +void fit2(const Vector3d* const ps, RSS& bv); //============================================================================== extern template FCL_EXPORT -void fit3(Vector3* ps, RSS& bv); +void fit3(const Vector3d* const ps, RSS& bv); //============================================================================== extern template FCL_EXPORT -void fit6(Vector3* ps, RSS& bv); +void fit6(const Vector3d* const ps, RSS& bv); //============================================================================== extern template FCL_EXPORT -void fitn(Vector3* ps, int n, RSS& bv); +void fitn(const Vector3d* const ps, int n, RSS& bv); //============================================================================== } // namespace RSS_fit_functions @@ -298,7 +298,7 @@ namespace kIOS_fit_functions { //============================================================================== template FCL_EXPORT -void fit1(Vector3* ps, kIOS& bv) +void fit1(const Vector3* const ps, kIOS& bv) { bv.num_spheres = 1; bv.spheres[0].o = ps[0]; @@ -312,7 +312,7 @@ void fit1(Vector3* ps, kIOS& bv) //============================================================================== template FCL_EXPORT -void fit2(Vector3* ps, kIOS& bv) +void fit2(const Vector3* const ps, kIOS& bv) { bv.num_spheres = 5; @@ -350,7 +350,7 @@ void fit2(Vector3* ps, kIOS& bv) //============================================================================== template FCL_EXPORT -void fit3(Vector3* ps, kIOS& bv) +void fit3(const Vector3* const ps, kIOS& bv) { bv.num_spheres = 3; @@ -396,7 +396,7 @@ void fit3(Vector3* ps, kIOS& bv) //============================================================================== template FCL_EXPORT -void fitn(Vector3* ps, int n, kIOS& bv) +void fitn(const Vector3* const ps, int n, kIOS& bv) { Matrix3 M; Matrix3 E; @@ -464,22 +464,22 @@ void fitn(Vector3* ps, int n, kIOS& bv) //============================================================================== extern template FCL_EXPORT -void fit1(Vector3* ps, kIOS& bv); +void fit1(const Vector3d* const ps, kIOS& bv); //============================================================================== extern template FCL_EXPORT -void fit2(Vector3* ps, kIOS& bv); +void fit2(const Vector3d* const ps, kIOS& bv); //============================================================================== extern template FCL_EXPORT -void fit3(Vector3* ps, kIOS& bv); +void fit3(const Vector3d* const ps, kIOS& bv); //============================================================================== extern template FCL_EXPORT -void fitn(Vector3* ps, int n, kIOS& bv); +void fitn(const Vector3d* const ps, int n, kIOS& bv); //============================================================================== } // namespace kIOS_fit_functions @@ -492,7 +492,7 @@ namespace OBBRSS_fit_functions { //============================================================================== template FCL_EXPORT -void fit1(Vector3* ps, OBBRSS& bv) +void fit1(const Vector3* const ps, OBBRSS& bv) { OBB_fit_functions::fit1(ps, bv.obb); RSS_fit_functions::fit1(ps, bv.rss); @@ -501,7 +501,7 @@ void fit1(Vector3* ps, OBBRSS& bv) //============================================================================== template FCL_EXPORT -void fit2(Vector3* ps, OBBRSS& bv) +void fit2(const Vector3* const ps, OBBRSS& bv) { OBB_fit_functions::fit2(ps, bv.obb); RSS_fit_functions::fit2(ps, bv.rss); @@ -510,7 +510,7 @@ void fit2(Vector3* ps, OBBRSS& bv) //============================================================================== template FCL_EXPORT -void fit3(Vector3* ps, OBBRSS& bv) +void fit3(const Vector3* const ps, OBBRSS& bv) { OBB_fit_functions::fit3(ps, bv.obb); RSS_fit_functions::fit3(ps, bv.rss); @@ -519,7 +519,7 @@ void fit3(Vector3* ps, OBBRSS& bv) //============================================================================== template FCL_EXPORT -void fitn(Vector3* ps, int n, OBBRSS& bv) +void fitn(const Vector3* const ps, int n, OBBRSS& bv) { OBB_fit_functions::fitn(ps, n, bv.obb); RSS_fit_functions::fitn(ps, n, bv.rss); @@ -527,19 +527,19 @@ void fitn(Vector3* ps, int n, OBBRSS& bv) //============================================================================== extern template -void fit1(Vector3* ps, OBBRSS& bv); +void fit1(const Vector3d* const ps, OBBRSS& bv); //============================================================================== extern template -void fit2(Vector3* ps, OBBRSS& bv); +void fit2(const Vector3d* const ps, OBBRSS& bv); //============================================================================== extern template -void fit3(Vector3* ps, OBBRSS& bv); +void fit3(const Vector3d* const ps, OBBRSS& bv); //============================================================================== extern template -void fitn(Vector3* ps, int n, OBBRSS& bv); +void fitn(const Vector3d* const ps, int n, OBBRSS& bv); //============================================================================== } // namespace OBBRSS_fit_functions @@ -549,7 +549,7 @@ void fitn(Vector3* ps, int n, OBBRSS& bv); template struct FCL_EXPORT Fitter { - static void fit(Vector3* ps, int n, BV& bv) + static void fit(const Vector3* const ps, int n, BV& bv) { for(int i = 0; i < n; ++i) bv += ps[i]; @@ -560,7 +560,7 @@ struct FCL_EXPORT Fitter template struct FCL_EXPORT Fitter> { - static void fit(Vector3* ps, int n, OBB& bv) + static void fit(const Vector3* const ps, int n, OBB& bv) { switch(n) { @@ -586,7 +586,7 @@ struct FCL_EXPORT Fitter> template struct FCL_EXPORT Fitter> { - static void fit(Vector3* ps, int n, RSS& bv) + static void fit(const Vector3* const ps, int n, RSS& bv) { switch(n) { @@ -609,7 +609,7 @@ struct FCL_EXPORT Fitter> template struct FCL_EXPORT Fitter> { - static void fit(Vector3* ps, int n, kIOS& bv) + static void fit(const Vector3* const ps, int n, kIOS& bv) { switch(n) { @@ -632,7 +632,7 @@ struct FCL_EXPORT Fitter> template struct FCL_EXPORT Fitter> { - static void fit(Vector3* ps, int n, OBBRSS& bv) + static void fit(const Vector3* const ps, int n, OBBRSS& bv) { switch(n) { @@ -674,7 +674,7 @@ struct Fitter>; //============================================================================== template FCL_EXPORT -void fit(Vector3* ps, int n, BV& bv) +void fit(const Vector3* const ps, int n, BV& bv) { detail::Fitter::fit(ps, n, bv); } diff --git a/include/fcl/math/bv/utility.h b/include/fcl/math/bv/utility.h index fa033db0a..2cea40cad 100644 --- a/include/fcl/math/bv/utility.h +++ b/include/fcl/math/bv/utility.h @@ -47,7 +47,7 @@ namespace fcl /// @brief Compute a bounding volume that fits a set of n points. template FCL_EXPORT -void fit(Vector3* ps, int n, BV& bv); +void fit(const Vector3* const ps, int n, BV& bv); /// @brief Convert a bounding volume of type BV1 in configuration tf1 to /// bounding volume of type BV2 in identity configuration. diff --git a/include/fcl/math/geometry-inl.h b/include/fcl/math/geometry-inl.h index b66d64559..2aa5ffa74 100644 --- a/include/fcl/math/geometry-inl.h +++ b/include/fcl/math/geometry-inl.h @@ -80,8 +80,8 @@ void generateCoordinateSystem(Transform3d& tf); //============================================================================== extern template void getRadiusAndOriginAndRectangleSize( - Vector3d* ps, - Vector3d* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -93,8 +93,8 @@ void getRadiusAndOriginAndRectangleSize( //============================================================================== extern template void getRadiusAndOriginAndRectangleSize( - Vector3d* ps, - Vector3d* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -114,8 +114,8 @@ void circumCircleComputation( //============================================================================== extern template double maximumDistance( - Vector3d* ps, - Vector3d* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -124,8 +124,8 @@ double maximumDistance( //============================================================================== extern template void getExtentAndCenter( - Vector3d* ps, - Vector3d* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -136,8 +136,8 @@ void getExtentAndCenter( //============================================================================== extern template void getCovariance( - Vector3d* ps, - Vector3d* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, Triangle* ts, unsigned int* indices, int n, Matrix3d& M); @@ -150,8 +150,8 @@ namespace detail { template FCL_EXPORT S maximumDistance_mesh( - Vector3* ps, - Vector3* ps2, + const Vector3* const ps, + const Vector3* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -195,8 +195,8 @@ S maximumDistance_mesh( template FCL_EXPORT S maximumDistance_pointcloud( - Vector3* ps, - Vector3* ps2, + const Vector3* const ps, + const Vector3* const ps2, unsigned int* indices, int n, const Vector3& query) @@ -230,8 +230,8 @@ S maximumDistance_pointcloud( template FCL_EXPORT void getExtentAndCenter_pointcloud( - Vector3* ps, - Vector3* ps2, + const Vector3* const ps, + const Vector3* const ps2, unsigned int* indices, int n, const Matrix3& axis, @@ -295,8 +295,8 @@ void getExtentAndCenter_pointcloud( template FCL_EXPORT void getExtentAndCenter_mesh( - Vector3* ps, - Vector3* ps2, + const Vector3* const ps, + const Vector3* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -367,8 +367,8 @@ void getExtentAndCenter_mesh( //============================================================================== extern template double maximumDistance_mesh( - Vector3d* ps, - Vector3d* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -377,8 +377,8 @@ double maximumDistance_mesh( //============================================================================== extern template double maximumDistance_pointcloud( - Vector3d* ps, - Vector3d* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, unsigned int* indices, int n, const Vector3d& query); @@ -386,25 +386,25 @@ double maximumDistance_pointcloud( //============================================================================== extern template void getExtentAndCenter_pointcloud( - Vector3* ps, - Vector3* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, unsigned int* indices, int n, - const Matrix3& axis, - Vector3& center, - Vector3& extent); + const Matrix3d& axis, + Vector3d& center, + Vector3d& extent); //============================================================================== extern template void getExtentAndCenter_mesh( - Vector3* ps, - Vector3* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, Triangle* ts, unsigned int* indices, int n, - const Matrix3& axis, - Vector3& center, - Vector3& extent); + const Matrix3d& axis, + Vector3d& center, + Vector3d& extent); //============================================================================== } // namespace detail @@ -826,8 +826,8 @@ void relativeTransform( template FCL_EXPORT void getRadiusAndOriginAndRectangleSize( - Vector3* ps, - Vector3* ps2, + const Vector3* const ps, + const Vector3* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -1109,8 +1109,8 @@ void getRadiusAndOriginAndRectangleSize( template FCL_EXPORT void getRadiusAndOriginAndRectangleSize( - Vector3* ps, - Vector3* ps2, + const Vector3* const ps, + const Vector3* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -1416,8 +1416,8 @@ void circumCircleComputation( template FCL_EXPORT S maximumDistance( - Vector3* ps, - Vector3* ps2, + const Vector3* const ps, + const Vector3* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -1433,8 +1433,8 @@ S maximumDistance( template FCL_EXPORT void getExtentAndCenter( - Vector3* ps, - Vector3* ps2, + const Vector3* const ps, + const Vector3* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -1451,8 +1451,9 @@ void getExtentAndCenter( //============================================================================== template FCL_EXPORT -void getCovariance(Vector3* ps, - Vector3* ps2, +void getCovariance( + const Vector3* const ps, + const Vector3* const ps2, Triangle* ts, unsigned int* indices, int n, Matrix3& M) diff --git a/include/fcl/math/geometry.h b/include/fcl/math/geometry.h index aac140851..c119c749f 100644 --- a/include/fcl/math/geometry.h +++ b/include/fcl/math/geometry.h @@ -125,8 +125,8 @@ void relativeTransform( template FCL_EXPORT void getRadiusAndOriginAndRectangleSize( - Vector3* ps, - Vector3* ps2, + const Vector3* const ps, + const Vector3* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -140,8 +140,8 @@ void getRadiusAndOriginAndRectangleSize( template FCL_EXPORT void getRadiusAndOriginAndRectangleSize( - Vector3* ps, - Vector3* ps2, + const Vector3* const ps, + const Vector3* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -163,8 +163,8 @@ void circumCircleComputation( template FCL_EXPORT S maximumDistance( - Vector3* ps, - Vector3* ps2, + const Vector3* const ps, + const Vector3* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -175,8 +175,8 @@ S maximumDistance( template FCL_EXPORT void getExtentAndCenter( - Vector3* ps, - Vector3* ps2, + const Vector3* const ps, + const Vector3* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -189,8 +189,8 @@ void getExtentAndCenter( template FCL_EXPORT void getExtentAndCenter( - Vector3* ps, - Vector3* ps2, + const Vector3* const ps, + const Vector3* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -203,8 +203,8 @@ void getExtentAndCenter( template FCL_EXPORT void getCovariance( - Vector3* ps, - Vector3* ps2, + const Vector3* const ps, + const Vector3* const ps2, Triangle* ts, unsigned int* indices, int n, diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h index e716fc5cf..b39f08825 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h @@ -39,6 +39,11 @@ #define FCL_NARROWPHASE_DETAIL_GJKLIBCCD_INL_H #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h" +#include "fcl/narrowphase/detail/failed_at_this_configuration.h" + +#include +#include +#include #include "fcl/common/unused.h" #include "fcl/common/warning.h" @@ -223,53 +228,162 @@ _ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b, ccdVec3Cross(d, &e, c); } -static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir) -{ - const ccd_support_t *A, *B; - ccd_vec3_t AB, AO, tmp; - ccd_real_t dot; - - // get last added as A - A = ccdSimplexLast(simplex); - // get the other point - B = ccdSimplexPoint(simplex, 0); - // compute AB oriented segment - ccdVec3Sub2(&AB, &B->v, &A->v); - // compute AO vector - ccdVec3Copy(&AO, &A->v); - ccdVec3Scale(&AO, -CCD_ONE); - - // dot product AB . AO - dot = ccdVec3Dot(&AB, &AO); - - // check if origin doesn't lie on AB segment - ccdVec3Cross(&tmp, &AB, &AO); - if (ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO){ +/* This is *not* an implementation of the general function: what's the nearest + point on the line segment AB to the origin O? It is not intended to be. + This is a limited, special case which exploits the known (or at least + expected) construction history of AB. The history is as follows: + + 1. We originally started with the Minkowski support point B (p_OB), which + was *not* the origin. + 2. We define a support direction as p_BO = -p_OB and use that to get the + Minkowski support point A. + 3. We confirm that O is not strictly beyond A in the direction p_BO + (confirming separation). + 4. Then, and only then, do we invoke this method. + + The method will do one of two things: + + - determine if the origin lies within the simplex (i.e. lies on the line + segment, confirming non-separation) and reports if this is the case, + - otherwise it computes a new support direction: a vector pointing to the + origin from the nearest point on the segment AB. The direction is + guaranteed; the only guarantee about the magnitude is that it is + numerically viable (i.e. greater than epsilon). + + The algorithm exploits the construction history as outlined below. Without + loss of generality, we place B some non-zero distance away from the origin + along the î direction (all other orientations can be rigidly transformed to + this canonical example). The diagram below shows the origin O and the point + B. It also shows three regions: 1, 2, and 3. + + ĵ + 1 ⯅ 2 3 + │░░░░░░░░░░┆▒▒▒▒▒ + │░░░░░░░░░░┆▒▒▒▒▒ + │░░░░░░░░░░┆▒▒▒▒▒ + │░░░░░░░░░░┆▒▒▒▒▒ + │░░░░░░░░░░┆▒▒▒▒▒ + ───────────O──────────B────⯈ î + │░░░░░░░░░░┆▒▒▒▒▒ + │░░░░░░░░░░┆▒▒▒▒▒ + │░░░░░░░░░░┆▒▒▒▒▒ + │░░░░░░░░░░┆▒▒▒▒▒ + │░░░░░░░░░░┆▒▒▒▒▒ + + The point A cannot lie in region 3. + + - B is a support point of the Minkowski difference. p_BO defines the + support vector that produces the support point A. Vertex A must lie at + least as far in the p_BO as B otherwise A is not actually a valid support + point for that direction. It could lie on the boundary of regions 2 & 3 + and still be a valid support point. + + The point A cannot lie in region 2 (or on the boundary between 2 & 3). + - We confirm that O is not strictly beyond A in the direction p_BO. For all + A in region 2, O lies beyond A (when projected onto the p_BO vector). + + The point A _must_ lie in region 1 (including the boundary between regions 1 & + 2) by process of elimination. + + The implication of this is that the O must project onto the _interior_ of the + line segment AB (with one notable exception). If A = O, then the projection of + O is at the end point and, is in fact, itself. + + Therefore, this function can only have two possible outcomes: + + 1. In the case where p_BA = k⋅p_BO (i.e., they are co-linear), we know the + origin lies "in" the simplex. If A = O, it lies on the simplex's surface + and the objects are touching, otherwise, the objects are penetrating. + Either way, we can report that they are definitely *not* separated. + 2. p_BA ≠ k⋅p_BO, we define the new support direction as perpendicular to the + line segment AB, pointing to O from the nearest point on the segment to O. + + Return value indicates concrete knowledge that the origin lies "in" the + 2-simplex (encoded as a 1), or indication that computation should continue (0). + + @note: doSimplex2 should _only_ be called with the construction history + outlined above: promotion of a 1-simplex. doSimplex2() is only invoked by + doSimplex(). This follows the computation of A and the promotion of the + simplex. Therefore, the history is always valid. Even though doSimplex3() can + demote itself to a 2-simplex, that 2-simplex immediately gets promoted back to + a 3-simplex via the same construction process. Therefore, as long as + doSimplex2() is only called from doSimplex() its region 1 assumption _should_ + remain valid. +*/ +static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir) { + // Used to define numerical thresholds near zero; typically scaled to the size + // of the quantities being tested. + const ccd_real_t eps = constants::eps(); + + const Vector3 p_OA(simplex->ps[simplex->last].v.v); + const Vector3 p_OB(simplex->ps[0].v.v); + + // Confirm that A is in region 1. Given that A may be very near to the origin, + // we must avoid normalizing p_OA. So, we use this instead. + // let A' be the projection of A onto the line defined by O and B. + // |A'B| >= |OB| iff A is in region 1. + // Numerically, we can express it as follows (allowing for |A'B| to be ever + // so slightly *less* than |OB|): + // p_AB ⋅ phat_OB >= |p_OB| - |p_OB| * ε = |p_OB| * (1 - ε) + // p_AB ⋅ phat_OB ⋅ |p_OB| >= |p_OB|⋅|p_OB| * (1 - ε) + // p_AB ⋅ p_OB >= |p_OB|² * (1 - ε) + // (p_OB - p_OA) ⋅ p_OB >= |p_OB|² * (1 - ε) + // p_OB ⋅ p_OB - p_OA ⋅ p_OB >= |p_OB|² * (1 - ε) + // |p_OB|² - p_OA ⋅ p_OB >= |p_OB|² * (1 - ε) + // -p_OA ⋅ p_OB >= -|p_OB|²ε + // p_OA ⋅ p_OB <= |p_OB|²ε + assert(p_OA.dot(p_OB) <= p_OB.squaredNorm() * eps && "A is not in region 1"); + + // Test for co-linearity. Given A is in region 1, co-linearity --> O is "in" + // the simplex. + // We'll use the angle between two vectors to determine co-linearity: p_AB + // and p_OB. If they are co-linear, then the angle between them (θ) is zero. + // Similarly, sin(θ) is zero. Ideally, it can be expressed as: + // |p_AB × p_OB| = |p_AB||p_OB||sin(θ)| = 0 + // Numerically, we allow θ (and sin(θ)) to be slightly larger than zero + // leading to a numerical formulation as: + // |p_AB × p_OB| = |p_AB||p_OB||sin(θ)| < |p_AB||p_OB|ε + // Finally, to reduce the computational cost, we eliminate the square roots by + // evaluating the equivalently discriminating test: + // |p_AB × p_OB|² < |p_AB|²|p_OB|²ε² + // + // In addition to providing a measure of co-linearity, the cross product gives + // us the normal to the plane on which points A, B, and O lie (which we will + // use later to compute a new support direction, as necessary). + const Vector3 p_AB = p_OB - p_OA; + const Vector3 plane_normal = p_OB.cross(p_AB); + if (plane_normal.squaredNorm() < + p_AB.squaredNorm() * p_OB.squaredNorm() * eps * eps) { return 1; } - // check if origin is in area where AB segment is - if (ccdIsZero(dot) || dot < CCD_ZERO){ - // origin is in outside are of A - ccdSimplexSet(simplex, 0, A); - ccdSimplexSetSize(simplex, 1); - ccdVec3Copy(dir, &AO); - }else{ - // origin is in area where AB segment is - - // keep simplex untouched and set direction to - // AB x AO x AB - tripleCross(&AB, &AO, &AB, dir); - } - + // O is not co-linear with AB, so dist(O, AB) > ε. Define `dir` as the + // direction to O from the nearest point on AB. + // Note: We use the normalized `plane_normal` (n̂) because we've already + // concluded that the origin is farther from AB than ε. We want to make sure + // `dir` likewise has a magnitude larger than ε. With normalization, we know + // |dir| = |n̂ × AB| = |AB| > dist(O, AB) > ε. + // Without normalizing, if |OA| and |OB| were smaller than ³√ε but + // sufficiently larger than ε, dist(O, AB) > ε, but |dir| < ε. + const Vector3 new_dir = plane_normal.normalized().cross(p_AB); + ccdVec3Set(dir, new_dir(0), new_dir(1), new_dir(2)); return 0; } + static bool isAbsValueLessThanEpsSquared(ccd_real_t val) { + return std::abs(val) < std::numeric_limits::epsilon() * + std::numeric_limits::epsilon(); + } + +// TODO(SeanCurtis-TRI): Define the return value: +// 1: (like doSimplex2) --> origin is "in" the simplex. +// 0: +// -1: If the 3-simplex is degenerate. How is this intepreted? static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir) { const ccd_support_t *A, *B, *C; ccd_vec3_t AO, AB, AC, ABC, tmp; - ccd_real_t dot, dist; + ccd_real_t dot; // get last added as A A = ccdSimplexLast(simplex); @@ -278,14 +392,25 @@ static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir) C = ccdSimplexPoint(simplex, 0); // check touching contact - dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, nullptr); - if (ccdIsZero(dist)){ + // Compute origin_projection as well. Without computing the origin projection, + // libccd could give inaccurate result. See + // https://github.com/danfis/libccd/issues/55. + ccd_vec3_t origin_projection_unused; + + const ccd_real_t dist_squared = ccdVec3PointTriDist2( + ccd_vec3_origin, &A->v, &B->v, &C->v, &origin_projection_unused); + if (isAbsValueLessThanEpsSquared(dist_squared)) { return 1; } // check if triangle is really triangle (has area > 0) - // if not simplex can't be expanded and thus no itersection is found + // if not simplex can't be expanded and thus no intersection is found + // TODO(SeanCurtis-TRI): Coincident points is sufficient but not necessary + // for a zero-area triangle. What about co-linearity? Can we guarantee that + // co-linearity can't happen? See the `triangle_area_is_zero()` method in + // this same file. if (ccdVec3Eq(&A->v, &B->v) || ccdVec3Eq(&A->v, &C->v)){ + // TODO(SeanCurtis-TRI): Why do we simply quit if the simplex is degenerate? return -1; } @@ -351,7 +476,6 @@ static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir) ccd_vec3_t AO, AB, AC, AD, ABC, ACD, ADB; int B_on_ACD, C_on_ADB, D_on_ABC; int AB_O, AC_O, AD_O; - ccd_real_t dist; // get last added as A A = ccdSimplexLast(simplex); @@ -362,26 +486,32 @@ static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir) // check if tetrahedron is really tetrahedron (has volume > 0) // if it is not simplex can't be expanded and thus no intersection is - // found - dist = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, nullptr); - if (ccdIsZero(dist)){ + // found. + // point_projection_on_triangle_unused is not used. We ask + // ccdVec3PointTriDist2 to compute this witness point, so as to get a + // numerical robust dist_squared. See + // https://github.com/danfis/libccd/issues/55 for an explanation. + ccd_vec3_t point_projection_on_triangle_unused; + ccd_real_t dist_squared = ccdVec3PointTriDist2( + &A->v, &B->v, &C->v, &D->v, &point_projection_on_triangle_unused); + if (isAbsValueLessThanEpsSquared(dist_squared)) { return -1; } // check if origin lies on some of tetrahedron's face - if so objects // intersect - dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, nullptr); - if (ccdIsZero(dist)) - return 1; - dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, nullptr); - if (ccdIsZero(dist)) - return 1; - dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, nullptr); - if (ccdIsZero(dist)) - return 1; - dist = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, nullptr); - if (ccdIsZero(dist)) - return 1; + dist_squared = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, + &point_projection_on_triangle_unused); + if (isAbsValueLessThanEpsSquared((dist_squared))) return 1; + dist_squared = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, + &point_projection_on_triangle_unused); + if (isAbsValueLessThanEpsSquared((dist_squared))) return 1; + dist_squared = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, + &point_projection_on_triangle_unused); + if (isAbsValueLessThanEpsSquared(dist_squared)) return 1; + dist_squared = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, + &point_projection_on_triangle_unused); + if (isAbsValueLessThanEpsSquared(dist_squared)) return 1; // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors ccdVec3Copy(&AO, &A->v); @@ -555,90 +685,136 @@ static int simplexToPolytope2(const void *obj1, const void *obj2, return 0; } - -/** Transforms simplex to polytope, three vertices required */ -static int simplexToPolytope3(const void *obj1, const void *obj2, - const ccd_t *ccd, - const ccd_simplex_t *simplex, - ccd_pt_t *pt, ccd_pt_el_t **nearest) -{ - const ccd_support_t *a, *b, *c; - ccd_support_t d, d2; - ccd_vec3_t ab, ac, dir; - ccd_pt_vertex_t *v[5]; - ccd_pt_edge_t *e[9]; - ccd_real_t dist, dist2; - - *nearest = NULL; - - a = ccdSimplexPoint(simplex, 0); - b = ccdSimplexPoint(simplex, 1); - c = ccdSimplexPoint(simplex, 2); - - // If only one triangle left from previous GJK run origin lies on this - // triangle. So it is necessary to expand triangle into two - // tetrahedrons connected with base (which is exactly abc triangle). - - // get next support point in direction of normal of triangle - ccdVec3Sub2(&ab, &b->v, &a->v); - ccdVec3Sub2(&ac, &c->v, &a->v); - ccdVec3Cross(&dir, &ab, &ac); - __ccdSupport(obj1, obj2, &dir, ccd, &d); - dist = ccdVec3PointTriDist2(&d.v, &a->v, &b->v, &c->v, NULL); - - // and second one take in opposite direction - ccdVec3Scale(&dir, -CCD_ONE); - __ccdSupport(obj1, obj2, &dir, ccd, &d2); - dist2 = ccdVec3PointTriDist2(&d2.v, &a->v, &b->v, &c->v, NULL); - - // check if face isn't already on edge of minkowski sum and thus we - // have touching contact - if (ccdIsZero(dist) || ccdIsZero(dist2)){ - v[0] = ccdPtAddVertex(pt, a); - v[1] = ccdPtAddVertex(pt, b); - v[2] = ccdPtAddVertex(pt, c); - e[0] = ccdPtAddEdge(pt, v[0], v[1]); - e[1] = ccdPtAddEdge(pt, v[1], v[2]); - e[2] = ccdPtAddEdge(pt, v[2], v[0]); - *nearest = (ccd_pt_el_t *)ccdPtAddFace(pt, e[0], e[1], e[2]); - if (*nearest == NULL) - return -2; - - return -1; +#ifndef NDEBUG +static bool isPolytopeEmpty(const ccd_pt_t& polytope) { + ccd_pt_vertex_t* v = nullptr; + ccdListForEachEntry(&polytope.vertices, v, ccd_pt_vertex_t, list) { + if (v) { + return false; } + } + ccd_pt_edge_t* e = nullptr; + ccdListForEachEntry(&polytope.edges, e, ccd_pt_edge_t, list) { + if (e) { + return false; + } + } + ccd_pt_face_t* f = nullptr; + ccdListForEachEntry(&polytope.faces, f, ccd_pt_face_t, list) { + if (f) { + return false; + } + } + return true; +} +#endif - // form polyhedron - v[0] = ccdPtAddVertex(pt, a); - v[1] = ccdPtAddVertex(pt, b); - v[2] = ccdPtAddVertex(pt, c); - v[3] = ccdPtAddVertex(pt, &d); - v[4] = ccdPtAddVertex(pt, &d2); - - e[0] = ccdPtAddEdge(pt, v[0], v[1]); - e[1] = ccdPtAddEdge(pt, v[1], v[2]); - e[2] = ccdPtAddEdge(pt, v[2], v[0]); - - e[3] = ccdPtAddEdge(pt, v[3], v[0]); - e[4] = ccdPtAddEdge(pt, v[3], v[1]); - e[5] = ccdPtAddEdge(pt, v[3], v[2]); - - e[6] = ccdPtAddEdge(pt, v[4], v[0]); - e[7] = ccdPtAddEdge(pt, v[4], v[1]); - e[8] = ccdPtAddEdge(pt, v[4], v[2]); +/** Transforms a 2-simplex (triangle) to a polytope (tetrahedron), three + * vertices required. + * Both the simplex and the transformed polytope contain the origin. The simplex + * vertices lie on the surface of the Minkowski difference obj1 ⊖ obj2. + * @param[in] obj1 object 1 on which the distance is queried. + * @param[in] obj2 object 2 on which the distance is queried. + * @param[in] ccd The ccd solver. + * @param[in] simplex The simplex (with three vertices) that contains the + * origin. + * @param[out] polytope The polytope (tetrahedron) that also contains the origin + * on one of its faces. At input, the polytope should be empty. + * @param[out] nearest If the function detects that obj1 and obj2 are touching, + * then set *nearest to be the nearest points on obj1 and obj2 respectively; + * otherwise set *nearest to NULL. @note nearest cannot be NULL. + * @retval status return 0 on success, -1 if touching contact is detected, and + * -2 on non-recoverable failure (mostly due to memory allocation bug). + */ +static int convert2SimplexToTetrahedron(const void* obj1, const void* obj2, + const ccd_t* ccd, const ccd_simplex_t* simplex, + ccd_pt_t* polytope, ccd_pt_el_t** nearest) { + assert(nearest); + assert(isPolytopeEmpty(*polytope)); + assert(simplex->last == 2); // a 2-simplex. + const ccd_support_t *a, *b, *c; + ccd_support_t d, d2; + ccd_vec3_t ab, ac, dir; + ccd_pt_vertex_t* v[4]; + ccd_pt_edge_t* e[6]; + + *nearest = NULL; + + a = ccdSimplexPoint(simplex, 0); + b = ccdSimplexPoint(simplex, 1); + c = ccdSimplexPoint(simplex, 2); + + // The 2-simplex is just a triangle containing the origin. We will expand this + // triangle to a tetrahedron, by adding the support point along the normal + // direction of the triangle. + ccdVec3Sub2(&ab, &b->v, &a->v); + ccdVec3Sub2(&ac, &c->v, &a->v); + ccdVec3Cross(&dir, &ab, &ac); + __ccdSupport(obj1, obj2, &dir, ccd, &d); + ccd_vec3_t point_projection_on_triangle_unused; + const ccd_real_t dist_squared = ccdVec3PointTriDist2( + &d.v, &a->v, &b->v, &c->v, &point_projection_on_triangle_unused); + + // and second one take in opposite direction + ccdVec3Scale(&dir, -CCD_ONE); + __ccdSupport(obj1, obj2, &dir, ccd, &d2); + const ccd_real_t dist_squared_opposite = ccdVec3PointTriDist2( + &d2.v, &a->v, &b->v, &c->v, &point_projection_on_triangle_unused); + + // check if face isn't already on edge of minkowski sum and thus we + // have touching contact + if (ccdIsZero(dist_squared) || ccdIsZero(dist_squared_opposite)) { + v[0] = ccdPtAddVertex(polytope, a); + v[1] = ccdPtAddVertex(polytope, b); + v[2] = ccdPtAddVertex(polytope, c); + e[0] = ccdPtAddEdge(polytope, v[0], v[1]); + e[1] = ccdPtAddEdge(polytope, v[1], v[2]); + e[2] = ccdPtAddEdge(polytope, v[2], v[0]); + *nearest = (ccd_pt_el_t*)ccdPtAddFace(polytope, e[0], e[1], e[2]); + if (*nearest == NULL) return -2; - if (ccdPtAddFace(pt, e[3], e[4], e[0]) == NULL - || ccdPtAddFace(pt, e[4], e[5], e[1]) == NULL - || ccdPtAddFace(pt, e[5], e[3], e[2]) == NULL + return -1; + } + // Form a tetrahedron with abc as one face, pick either d or d2, based + // on which one has larger distance to the face abc. We pick the larger + // distance because it gives a tetrahedron with larger volume, so potentially + // more "expanded" than the one with the smaller volume. + auto FormTetrahedron = [polytope, a, b, c, &v, + &e](const ccd_support_t& new_support) -> int { + v[0] = ccdPtAddVertex(polytope, a); + v[1] = ccdPtAddVertex(polytope, b); + v[2] = ccdPtAddVertex(polytope, c); + v[3] = ccdPtAddVertex(polytope, &new_support); + + e[0] = ccdPtAddEdge(polytope, v[0], v[1]); + e[1] = ccdPtAddEdge(polytope, v[1], v[2]); + e[2] = ccdPtAddEdge(polytope, v[2], v[0]); + e[3] = ccdPtAddEdge(polytope, v[0], v[3]); + e[4] = ccdPtAddEdge(polytope, v[1], v[3]); + e[5] = ccdPtAddEdge(polytope, v[2], v[3]); - || ccdPtAddFace(pt, e[6], e[7], e[0]) == NULL - || ccdPtAddFace(pt, e[7], e[8], e[1]) == NULL - || ccdPtAddFace(pt, e[8], e[6], e[2]) == NULL){ - return -2; + // ccdPtAdd*() functions return NULL either if the memory allocation + // failed of if any of the input pointers are NULL, so the bad + // allocation can be checked by the last calls of ccdPtAddFace() + // because the rest of the bad allocations eventually "bubble up" here + // Note, there is no requirement on the winding of the face, namely we do + // not guarantee if all f.e(0).cross(f.e(1)) points outward (or inward) for + // all the faces added below. + if (ccdPtAddFace(polytope, e[0], e[1], e[2]) == NULL || + ccdPtAddFace(polytope, e[3], e[4], e[0]) == NULL || + ccdPtAddFace(polytope, e[4], e[5], e[1]) == NULL || + ccdPtAddFace(polytope, e[5], e[3], e[2]) == NULL) { + return -2; } - return 0; -} + }; + if (std::abs(dist_squared) > std::abs(dist_squared_opposite)) { + return FormTetrahedron(d); + } else { + return FormTetrahedron(d2); + } +} /** Transforms simplex to polytope. It is assumed that simplex has 4 * vertices! */ @@ -660,13 +836,15 @@ static int simplexToPolytope4(const void *obj1, const void *obj2, d = ccdSimplexPoint(simplex, 3); // check if origin lies on some of tetrahedron's face - if so use - // simplexToPolytope3() + // convert2SimplexToTetrahedron() use_polytope3 = 0; dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &b->v, &c->v, NULL); if (ccdIsZero(dist)){ use_polytope3 = 1; } dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &c->v, &d->v, NULL); + // TODO(hongkai.dai@tri.global) Optimize this part, check whether these + // "if" branches are mutually exclusive. if (ccdIsZero(dist)){ use_polytope3 = 1; ccdSimplexSet(simplex, 1, c); @@ -687,7 +865,7 @@ static int simplexToPolytope4(const void *obj1, const void *obj2, if (use_polytope3){ ccdSimplexSetSize(simplex, 3); - return simplexToPolytope3(obj1, obj2, ccd, simplex, pt, nearest); + return convert2SimplexToTetrahedron(obj1, obj2, ccd, simplex, pt, nearest); } // no touching contact - simply create tetrahedron @@ -716,190 +894,633 @@ static int simplexToPolytope4(const void *obj1, const void *obj2, return 0; } +/** Reports true if p and q are coincident. */ +static bool are_coincident(const ccd_vec3_t& p, const ccd_vec3_t& q) { + // This uses a scale-dependent basis for determining coincidence. It examines + // each axis independently, and only, if all three axes are sufficiently + // close (relative to its own scale), are the two points considered + // coincident. + // + // For dimension i, two values are considered the same if: + // |pᵢ - qᵢ| <= ε·max(1, |pᵢ|, |qᵢ|) + // And the points are coincident if the previous condition holds for all + // `i ∈ {0, 1, 2}` (i.e. the x-, y-, *and* z-dimensions). + using std::abs; + using std::max; + + const ccd_real_t eps = constants::eps(); + // NOTE: Wrapping "1.0" with ccd_real_t accounts for mac problems where ccd + // is actually float based. + for (int i = 0; i < 3; ++i) { + const ccd_real_t tolerance = + max({ccd_real_t{1}, abs(p.v[i]), abs(q.v[i])}) * eps; + const ccd_real_t delta = abs(p.v[i] - q.v[i]); + if (delta > tolerance) return false; + } + return true; +} + +/** Determines if the the triangle defined by the three vertices has zero area. + Area can be zero for one of two reasons: + - the triangle is so small that the vertices are functionally coincident, or + - the vertices are co-linear. + Both conditions are computed with respect to machine precision. + @returns true if the area is zero. */ +static bool triangle_area_is_zero(const ccd_vec3_t& a, const ccd_vec3_t& b, + const ccd_vec3_t& c) { + // First coincidence condition. This doesn't *explicitly* test for b and c + // being coincident. That will be captured in the subsequent co-linearity + // test. If b and c *were* coincident, it would be cheaper to perform the + // coincidence test than the co-linearity test. + // However, the expectation is that typically the triangle will not have zero + // area. In that case, we want to minimize the number of tests performed on + // the average, so we prefer to eliminate one coincidence test. + if (are_coincident(a, b) || are_coincident(a, c)) return true; + + // We're going to compute the *sine* of the angle θ between edges (given that + // the vertices are *not* coincident). If the sin(θ) < ε, the edges are + // co-linear. + ccd_vec3_t AB, AC, n; + ccdVec3Sub2(&AB, &b, &a); + ccdVec3Sub2(&AC, &c, &a); + ccdVec3Normalize(&AB); + ccdVec3Normalize(&AC); + ccdVec3Cross(&n, &AB, &AC); + const ccd_real_t eps = constants::eps(); + // Second co-linearity condition. + if (ccdVec3Len2(&n) < eps * eps) return true; + return false; +} +/** + * Determines if the point P is on the line segment AB. + * If A, B, P are coincident, report true. + */ +static bool is_point_on_line_segment(const ccd_vec3_t& p, const ccd_vec3_t& a, + const ccd_vec3_t& b) { + if (are_coincident(a, b)) { + return are_coincident(a, p); + } + // A and B are not coincident, if the triangle ABP has non-zero area, then P + // is not on the line adjoining AB, and hence not on the line segment AB. + if (!triangle_area_is_zero(a, b, p)) { + return false; + } + // P is on the line adjoinging AB. If P is on the line segment AB, then + // PA.dot(PB) <= 0. + ccd_vec3_t PA, PB; + ccdVec3Sub2(&PA, &p, &a); + ccdVec3Sub2(&PB, &p, &b); + return ccdVec3Dot(&PA, &PB) <= 0; +} + +/** + * Computes the normal vector of a triangular face on a polytope, and the normal + * vector points outward from the polytope. Notice we assume that the origin + * lives within the polytope, and the normal vector may not have unit length. + * @param[in] polytope The polytope on which the face lives. We assume that the + * origin also lives inside the polytope. + * @param[in] face The face for which the normal vector is computed. + * @retval dir The vector normal to the face, and points outward from the + * polytope. `dir` is unnormalized, that it does not necessarily have a unit + * length. + * @throws UnexpectedConfigurationException if built in debug mode _and_ the + * triangle has zero area (either by being too small, or being co-linear). + */ +static ccd_vec3_t faceNormalPointingOutward(const ccd_pt_t* polytope, + const ccd_pt_face_t* face) { + // This doesn't necessarily define a triangle; I don't know that the third + // vertex added here is unique from the other two. +#ifndef NDEBUG + // quick test for degeneracy + const ccd_vec3_t& a = face->edge[0]->vertex[1]->v.v; + const ccd_vec3_t& b = face->edge[0]->vertex[0]->v.v; + const ccd_vec3_t& test_v = face->edge[1]->vertex[0]->v.v; + const ccd_vec3_t& c = are_coincident(test_v, a) || are_coincident(test_v, b) ? + face->edge[1]->vertex[1]->v.v : test_v; + if (triangle_area_is_zero(a, b, c)) { + FCL_THROW_FAILED_AT_THIS_CONFIGURATION( + "Cannot compute face normal for a degenerate (zero-area) triangle"); + } +#endif + // We find two edges of the triangle as e1 and e2, and the normal vector + // of the face is e1.cross(e2). + ccd_vec3_t e1, e2; + ccdVec3Sub2(&e1, &(face->edge[0]->vertex[1]->v.v), + &(face->edge[0]->vertex[0]->v.v)); + ccdVec3Sub2(&e2, &(face->edge[1]->vertex[1]->v.v), + &(face->edge[1]->vertex[0]->v.v)); + ccd_vec3_t dir; + // TODO(hongkai.dai): we ignore the degeneracy here, namely we assume e1 and + // e2 are not colinear. We should check if e1 and e2 are colinear, and handle + // this corner case. + ccdVec3Cross(&dir, &e1, &e2); + const ccd_real_t dir_norm = std::sqrt(ccdVec3Len2(&dir)); + ccd_vec3_t unit_dir = dir; + ccdVec3Scale(&unit_dir, 1.0 / dir_norm); + // The winding of the triangle is *not* guaranteed. The normal `n = e₁ × e₂` + // may point inside or outside. We rely on the fact that the origin lies + // within the polytope to resolve this ambiguity. A vector from the origin to + // any point on the triangle must point in the "same" direction as the normal + // (positive dot product). + + // However, the distance to the origin may be too small for the origin to + // serve as a reliable witness of inside-ness. In that case, we examine the + // polytope's *other* vertices; they should all lie on the "inside" of the + // current triangle. If at least one is a reliable distance, then that is + // considered to be the inside. If all vertices are "too close" (like the + // origin), then "inside" is defined as the side of the triangle that had the + // most distant vertex. + + // For these tests, we use the arbitrary distance of 0.01 unit length as a + // "reliable" distance for both the origin and other vertices. Even if it + // seems large, the fall through case of comparing the maximum distance will + // always guarantee correctness. + const ccd_real_t dist_tol = 0.01; + // origin_distance_to_plane computes the signed distance from the origin to + // the plane nᵀ * (x - v) = 0 coinciding with the triangle, where v is a + // point on the triangle. + const ccd_real_t origin_distance_to_plane = + ccdVec3Dot(&unit_dir, &(face->edge[0]->vertex[0]->v.v)); + if (origin_distance_to_plane < -dist_tol) { + // Origin is more than `dist_tol` away from the plane, but the negative + // value implies that the normal vector is pointing in the wrong direction; + // flip it. + ccdVec3Scale(&dir, ccd_real_t(-1)); + } else if (-dist_tol <= origin_distance_to_plane && + origin_distance_to_plane <= dist_tol) { + // The origin is close to the plane of the face. Pick another vertex to test + // the normal direction. + ccd_real_t max_distance_to_plane = -CCD_REAL_MAX; + ccd_real_t min_distance_to_plane = CCD_REAL_MAX; + ccd_pt_vertex_t* v; + // If the magnitude of the distance_to_plane is larger than dist_tol, + // then it means one of the vertices is at least `dist_tol` away from the + // plane coinciding with the face. + ccdListForEachEntry(&polytope->vertices, v, ccd_pt_vertex_t, list) { + // distance_to_plane is the signed distance from the + // vertex v->v.v to the face, i.e., distance_to_plane = nᵀ * + // (v->v.v - face_point). Note that origin_distance_to_plane = nᵀ * + // face_point. + const ccd_real_t distance_to_plane = + ccdVec3Dot(&unit_dir, &(v->v.v)) - origin_distance_to_plane; + if (distance_to_plane > dist_tol) { + // The vertex is at least dist_tol away from the face plane, on the same + // direction of `dir`. So we flip dir to point it outward from the + // polytope. + ccdVec3Scale(&dir, ccd_real_t(-1)); + return dir; + } else if (distance_to_plane < -dist_tol) { + // The vertex is at least `dist_tol` away from the face plane, on the + // opposite direction of `dir`. So `dir` points outward already. + return dir; + } else { + max_distance_to_plane = + std::max(max_distance_to_plane, distance_to_plane); + min_distance_to_plane = + std::min(min_distance_to_plane, distance_to_plane); + } + } + // If max_distance_to_plane > |min_distance_to_plane|, it means that the + // vertices that are on the positive side of the plane, has a larger maximal + // distance than the vertices on the negative side of the plane. Thus we + // regard that `dir` points into the polytope. Hence we flip `dir`. + if (max_distance_to_plane > std::abs(min_distance_to_plane)) { + ccdVec3Scale(&dir, ccd_real_t(-1)); + } + } + return dir; +} + +// Return true if the point `pt` is on the outward side of the half-plane, on +// which the triangle `f1 lives. Notice if the point `pt` is exactly on the +// half-plane, the return is false. +// @param f A triangle on a polytope. +// @param pt A point. +static bool isOutsidePolytopeFace(const ccd_pt_t* polytope, + const ccd_pt_face_t* f, const ccd_vec3_t* pt) { + ccd_vec3_t n = faceNormalPointingOutward(polytope, f); + // r_VP is the vector from a vertex V on the face `f`, to the point P `pt`. + ccd_vec3_t r_VP; + ccdVec3Sub2(&r_VP, pt, &(f->edge[0]->vertex[0]->v.v)); + return ccdVec3Dot(&n, &r_VP) > 0; +} + +#ifndef NDEBUG +// The function ComputeVisiblePatchRecursiveSanityCheck() is only called in the +// debug mode. In the release mode, this function is declared/defined but not +// used. Without this NDEBUG macro, the function will cause a -Wunused-function +// error on CI's release builds. +/** + * Reports true if the visible patch is valid. + * The invariant for computing the visible patch is that for each edge in the + * polytope, if both neighbouring faces are visible, then the edge is an + * internal edge; if only one neighbouring face is visible, then the edge + * is a border edge. + * For each face, if one of its edges is an internal edge, then the face is + * visible. + */ +static bool ComputeVisiblePatchRecursiveSanityCheck( + const ccd_pt_t& polytope, + const std::unordered_set& border_edges, + const std::unordered_set& visible_faces, + const std::unordered_set& internal_edges) { + ccd_pt_face_t* f; + ccdListForEachEntry(&polytope.faces, f, ccd_pt_face_t, list) { + bool has_edge_internal = false; + for (int i = 0; i < 3; ++i) { + // Since internal_edges is a set, internal_edges.count(e) means that e + // is contained in the set internal_edges. + if (internal_edges.count(f->edge[i]) > 0) { + has_edge_internal = true; + break; + } + } + if (has_edge_internal) { + if (visible_faces.count(f) == 0) { + return false; + } + } + } + ccd_pt_edge_t* e; + ccdListForEachEntry(&polytope.edges, e, ccd_pt_edge_t, list) { + if (visible_faces.count(e->faces[0]) > 0 && + visible_faces.count(e->faces[1]) > 0) { + if (internal_edges.count(e) == 0) { + return false; + } + } else if (visible_faces.count(e->faces[0]) + + visible_faces.count(e->faces[1]) == + 1) { + if (border_edges.count(e) == 0) { + return false; + } + } + } + return true; +} +#endif - -/** Expands polytope's tri by new vertex v. Triangle tri is replaced by - * three triangles each with one vertex in v. */ -static int expandPolytope(ccd_pt_t *pt, ccd_pt_el_t *el, - const ccd_support_t *newv) -{ - ccd_pt_vertex_t *v[5]; - ccd_pt_edge_t *e[8]; - ccd_pt_face_t *f[2]; - - - // element can be either segment or triangle - if (el->type == CCD_PT_EDGE){ - // In this case, segment should be replaced by new point. - // Simpliest case is when segment stands alone and in this case - // this segment is replaced by two other segments both connected to - // newv. - // Segment can be also connected to max two faces and in that case - // each face must be replaced by two other faces. To do this - // correctly it is necessary to have correctly ordered edges and - // vertices which is exactly what is done in following code. - // - - ccdPtEdgeVertices((const ccd_pt_edge_t *)el, &v[0], &v[2]); - - ccdPtEdgeFaces((ccd_pt_edge_t *)el, &f[0], &f[1]); - - if (f[0]){ - ccdPtFaceEdges(f[0], &e[0], &e[1], &e[2]); - if (e[0] == (ccd_pt_edge_t *)el){ - e[0] = e[2]; - }else if (e[1] == (ccd_pt_edge_t *)el){ - e[1] = e[2]; - } - ccdPtEdgeVertices(e[0], &v[1], &v[3]); - if (v[1] != v[0] && v[3] != v[0]){ - e[2] = e[0]; - e[0] = e[1]; - e[1] = e[2]; - if (v[1] == v[2]) - v[1] = v[3]; - }else{ - if (v[1] == v[0]) - v[1] = v[3]; - } - - if (f[1]){ - ccdPtFaceEdges(f[1], &e[2], &e[3], &e[4]); - if (e[2] == (ccd_pt_edge_t *)el){ - e[2] = e[4]; - }else if (e[3] == (ccd_pt_edge_t *)el){ - e[3] = e[4]; - } - ccdPtEdgeVertices(e[2], &v[3], &v[4]); - if (v[3] != v[2] && v[4] != v[2]){ - e[4] = e[2]; - e[2] = e[3]; - e[3] = e[4]; - if (v[3] == v[0]) - v[3] = v[4]; - }else{ - if (v[3] == v[2]) - v[3] = v[4]; - } - } - - - v[4] = ccdPtAddVertex(pt, newv); - - ccdPtDelFace(pt, f[0]); - if (f[1]){ - ccdPtDelFace(pt, f[1]); - ccdPtDelEdge(pt, (ccd_pt_edge_t *)el); - } - - e[4] = ccdPtAddEdge(pt, v[4], v[2]); - e[5] = ccdPtAddEdge(pt, v[4], v[0]); - e[6] = ccdPtAddEdge(pt, v[4], v[1]); - if (f[1]) - e[7] = ccdPtAddEdge(pt, v[4], v[3]); - - - if (ccdPtAddFace(pt, e[1], e[4], e[6]) == NULL - || ccdPtAddFace(pt, e[0], e[6], e[5]) == NULL){ - return -2; - } - - if (f[1]){ - FCL_SUPPRESS_MAYBE_UNINITIALIZED_BEGIN - if (ccdPtAddFace(pt, e[3], e[5], e[7]) == NULL - || ccdPtAddFace(pt, e[4], e[7], e[2]) == NULL){ - return -2; - } - FCL_SUPPRESS_MAYBE_UNINITIALIZED_END - }else{ - if (ccdPtAddFace(pt, e[4], e[5], (ccd_pt_edge_t *)el) == NULL) - return -2; - } - } - }else{ // el->type == CCD_PT_FACE - // replace triangle by tetrahedron without base (base would be the - // triangle that will be removed) - - // get triplet of surrounding edges and vertices of triangle face - ccdPtFaceEdges((const ccd_pt_face_t *)el, &e[0], &e[1], &e[2]); - ccdPtEdgeVertices(e[0], &v[0], &v[1]); - ccdPtEdgeVertices(e[1], &v[2], &v[3]); - - // following code sorts edges to have e[0] between vertices 0-1, - // e[1] between 1-2 and e[2] between 2-0 - if (v[2] != v[1] && v[3] != v[1]){ - // swap e[1] and e[2] - e[3] = e[1]; - e[1] = e[2]; - e[2] = e[3]; - } - if (v[3] != v[0] && v[3] != v[1]) - v[2] = v[3]; - - // remove triangle face - ccdPtDelFace(pt, (ccd_pt_face_t *)el); - - // expand triangle to tetrahedron - v[3] = ccdPtAddVertex(pt, newv); - e[3] = ccdPtAddEdge(pt, v[3], v[0]); - e[4] = ccdPtAddEdge(pt, v[3], v[1]); - e[5] = ccdPtAddEdge(pt, v[3], v[2]); - - if (ccdPtAddFace(pt, e[3], e[4], e[0]) == NULL - || ccdPtAddFace(pt, e[4], e[5], e[1]) == NULL - || ccdPtAddFace(pt, e[5], e[3], e[2]) == NULL){ - return -2; - } +/** + * This function contains the implementation detail of ComputeVisiblePatch() + * function. It should not be called by any function other than + * ComputeVisiblePatch(). + */ +static void ComputeVisiblePatchRecursive( + const ccd_pt_t& polytope, ccd_pt_face_t& f, int edge_index, + const ccd_vec3_t& query_point, + std::unordered_set* border_edges, + std::unordered_set* visible_faces, + std::unordered_set* internal_edges) { + /* + This function will be called recursively. It first checks if the face `g` + neighouring face `f` along the common edge `f->edge[edge_index]` can be seen + from the point `query_point`. If this face g cannot be seen, then stop. + Otherwise, we continue to check the neighbouring faces of g, by calling this + function recursively. + */ + ccd_pt_face_t* g = f.edge[edge_index]->faces[0] == &f + ? f.edge[edge_index]->faces[1] + : f.edge[edge_index]->faces[0]; + if (visible_faces->count(g) == 0) { + // g is not a visible face + if (!isOutsidePolytopeFace(&polytope, g, &query_point)) { + // Cannot see the neighbouring face from the new vertex. + + // TODO(hongkai.dai@tri.global): when the new vertex is colinear with a + // border edge, we should remove the degenerate triangle. We could remove + // the middle vertex on that line from the polytope, and then reconnect + // the polytope. + if (triangle_area_is_zero(query_point, f.edge[edge_index]->vertex[0]->v.v, + f.edge[edge_index]->vertex[1]->v.v)) { + FCL_THROW_FAILED_AT_THIS_CONFIGURATION( + "The new vertex is colinear with an existing edge. The added " + "triangle would be degenerate."); + } + border_edges->insert(f.edge[edge_index]); + return; + } + visible_faces->insert(g); + internal_edges->insert(f.edge[edge_index]); + for (int i = 0; i < 3; ++i) { + if (g->edge[i] != f.edge[edge_index]) { + // One of the neighbouring face is `f`, so do not need to visit again. + ComputeVisiblePatchRecursive(polytope, *g, i, query_point, border_edges, + visible_faces, internal_edges); + } } + } else { + // Face f is visible (prerequisite), and g has been previously + // marked visible (via a different path); their shared edge should be marked + // internal. + internal_edges->insert(f.edge[edge_index]); + } +} - return 0; +/** Defines the "visible" patch on the given convex `polytope` (with respect to + * the given `query_vertex` which is a point outside the polytope.) + * + * A patch is characterized by: + * - a contiguous set of visible faces + * - internal edges (the edges for which *both* adjacent faces are in the + * patch) + * - border edges (edges for which only *one* adjacent face is in the patch) + * + * A face `f` (with vertices `(v₀, v₁, v₂)` and outward pointing normal `n̂`) is + * "visible" and included in the patch if, for query vertex `q`: + * + * `n̂ ⋅ (q - v₀) > 0` + * + * Namely the query vertex `q` is on the "outer" side of the face `f`. + * + * @param polytope The polytope to evaluate. + * @param f A face known to be visible to the query point. + * @param query_point The point from which visibility is evaluated. + * @param[out] border_edges The collection of patch border edges. + * @param[out] visible_faces The collection of patch faces. + * @param[out] internal_edges The collection of internal edges. + * @throws UnexpectedConfigurationException in debug builds if the resulting + * patch is not consistent. + * + * @pre The `polytope` is convex. + * @pre The face `f` is visible from `query_point`. + * @pre Output parameters are non-null. + * TODO(hongkai.dai@tri.global) Replace patch computation with patch deletion + * and return border edges as an optimization. + * TODO(hongkai.dai@tri.global) Consider caching results of per-face visibility + * status to prevent redundant recalculation -- or by associating the face + * normal with the face. + */ +static void ComputeVisiblePatch( + const ccd_pt_t& polytope, ccd_pt_face_t& f, + const ccd_vec3_t& query_point, + std::unordered_set* border_edges, + std::unordered_set* visible_faces, + std::unordered_set* internal_edges) { + assert(border_edges); + assert(visible_faces); + assert(internal_edges); + assert(border_edges->empty()); + assert(visible_faces->empty()); + assert(internal_edges->empty()); + assert(isOutsidePolytopeFace(&polytope, &f, &query_point)); + visible_faces->insert(&f); + for (int edge_index = 0; edge_index < 3; ++edge_index) { + ComputeVisiblePatchRecursive(polytope, f, edge_index, query_point, + border_edges, visible_faces, internal_edges); + } +#ifndef NDEBUG + if (!ComputeVisiblePatchRecursiveSanityCheck( + polytope, *border_edges, *visible_faces, *internal_edges)) { + FCL_THROW_FAILED_AT_THIS_CONFIGURATION( + "The visible patch failed its sanity check"); + } +#endif } -/** Finds next support point (and stores it in out argument). - * Returns 0 on success, -1 otherwise */ -static int nextSupport(const void *obj1, const void *obj2, const ccd_t *ccd, - const ccd_pt_el_t *el, - ccd_support_t *out) +/** Expands the polytope by adding a new vertex `newv` to the polytope. The + * new polytope is the convex hull of the new vertex together with the old + * polytope. This new polytope includes new edges (by connecting the new vertex + * with existing vertices) and new faces (by connecting the new vertex with + * existing edges). We only keep the edges and faces that are on the boundary + * of the new polytope. The edges/faces on the original polytope that would be + * interior to the new convex hull are discarded. + * @param[in/out] polytope The polytope. + * @param[in] el A feature that is visible from the point `newv` and contains + * the polytope boundary point that is closest to the origin. This feature + * should be either a face or an edge. A face is visible from a point ouside the + * original polytope, if the point is on the "outer" side of the face. An edge + * is visible from that point, if one of its neighbouring faces is visible. This + * feature contains the point that is closest to the origin on the boundary of + * the polytope. If the feature is an edge, and the two neighbouring faces of + * that edge are not co-planar, then the origin must lie on that edge. The + * feature should not be a vertex, as that would imply the two objects are in + * touching contact, causing the algorithm to exit before calling + * expandPolytope() function. + * @param[in] newv The new vertex add to the polytope. + * @retval status Returns 0 on success. Returns -2 otherwise. + * @throws UnexpectedConfigurationException if expanding is meaningless either + * because 1) the nearest feature is a vertex, 2) the new vertex lies on + * an edge of the current polytope, or 3) the visible feature is an edge with + * one or more adjacent faces with no area. + */ +static int expandPolytope(ccd_pt_t *polytope, ccd_pt_el_t *el, + const ccd_support_t *newv) { - ccd_vec3_t *a, *b, *c; - ccd_real_t dist; - - if (el->type == CCD_PT_VERTEX) - return -1; - - // touch contact - if (ccdIsZero(el->dist)) - return -1; - - __ccdSupport(obj1, obj2, &el->witness, ccd, out); + // The outline of the algorithm is as follows: + // 1. Compute the visible patch relative to the new vertex (See + // ComputeVisiblePatch() for details). + // 2. Delete the faces and internal edges. + // 3. Build a new face from each border edge and the new vertex. + + // To remove all faces that can be seen from the new vertex, we start with the + // face on which the closest point lives, and then do a depth-first search on + // its neighbouring triangles, until the triangle cannot be seen from the new + // vertex. + // TODO(hongkai.dai@tri.global): it is inefficient to store visible + // faces/edges. A better implementation should remove visible faces and + // internal edges inside ComputeVisiblePatch() function, when traversing the + // faces on the polytope. We focus on the correctness in the first place. + // Later when we make sure that the whole EPA implementation is bug free, we + // will improve the performance. + + ccd_pt_face_t* start_face = NULL; + + if (el->type == CCD_PT_VERTEX) { + FCL_THROW_FAILED_AT_THIS_CONFIGURATION( + "The visible feature is a vertex. This should already have been " + "identified as a touching contact"); + } + // Start with the face on which the closest point lives + if (el->type == CCD_PT_FACE) { + start_face = reinterpret_cast(el); + } else if (el->type == CCD_PT_EDGE) { + // Check the two neighbouring faces of the edge. + ccd_pt_face_t* f[2]; + ccdPtEdgeFaces(reinterpret_cast(el), &f[0], &f[1]); + if (isOutsidePolytopeFace(polytope, f[0], &newv->v)) { + start_face = f[0]; + } else if (isOutsidePolytopeFace(polytope, f[1], &newv->v)) { + start_face = f[1]; + } else { + FCL_THROW_FAILED_AT_THIS_CONFIGURATION( + "Both the nearest point and the new vertex are on an edge, thus the " + "nearest distance should be 0. This is touching contact, and should " + "already have been identified"); + } + } - // Compute dist of support point along element witness point direction - // so we can determine whether we expanded a polytope surrounding the - // origin a bit. - dist = ccdVec3Dot(&out->v, &el->witness); + std::unordered_set visible_faces; + std::unordered_set internal_edges; + std::unordered_set border_edges; + ComputeVisiblePatch(*polytope, *start_face, newv->v, &border_edges, + &visible_faces, &internal_edges); + + // Now remove all the obsolete faces. + // TODO(hongkai.dai@tri.global): currently we need to loop through each face + // in visible_faces, and then do a linear search in the list pt->faces to + // delete `face`. It would be better if we only loop through the list + // polytope->faces for once. Same for the edges. + for (const auto& f : visible_faces) { + ccdPtDelFace(polytope, f); + } - if (dist - el->dist < ccd->epa_tolerance) - return -1; + // Now remove all the obsolete edges. + for (const auto& e : internal_edges) { + ccdPtDelEdge(polytope, e); + } - if (el->type == CCD_PT_EDGE){ - // fetch end points of edge - ccdPtEdgeVec3((ccd_pt_edge_t *)el, &a, &b); + // A vertex cannot be obsolete, since a vertex is always on the boundary of + // the Minkowski difference A ⊖ B. + // TODO(hongkai.dai@tri.global): as a sanity check, we should make sure that + // all vertices has at least one face/edge invisible from the new vertex + // `newv`. + + // Now add the new vertex. + ccd_pt_vertex_t* new_vertex = ccdPtAddVertex(polytope, newv); + + // Now add the new edges and faces, by connecting the new vertex with vertices + // on border_edges. map_vertex_to_new_edge maps a vertex on the silhouette + // edges to a new edge, with one end being the new vertex, and the other end + // being that vertex on the silhouette edges. + std::unordered_map map_vertex_to_new_edge; + for (const auto& border_edge : border_edges) { + ccd_pt_edge_t* e[2]; // The two new edges added by connecting new_vertex + // to the two vertices on border_edge. + for (int i = 0; i < 2; ++i) { + auto it = map_vertex_to_new_edge.find(border_edge->vertex[i]); + if (it == map_vertex_to_new_edge.end()) { + // This edge has not been added yet. + e[i] = ccdPtAddEdge(polytope, new_vertex, border_edge->vertex[i]); + map_vertex_to_new_edge.emplace_hint(it, border_edge->vertex[i], + e[i]); + } else { + e[i] = it->second; + } + } + // Now add the face. + ccdPtAddFace(polytope, border_edge, e[0], e[1]); + } - // get distance from segment - dist = ccdVec3PointSegmentDist2(&out->v, a, b, NULL); - }else{ // el->type == CCD_PT_FACE - // fetch vertices of triangle face - ccdPtFaceVec3((ccd_pt_face_t *)el, &a, &b, &c); + return 0; +} - // check if new point can significantly expand polytope - dist = ccdVec3PointTriDist2(&out->v, a, b, c, NULL); +/** In each iteration of EPA algorithm, given the nearest point on the polytope + * boundary to the origin, a support direction will be computed, to find the + * support of the Minkowski difference A ⊖ B along that direction, so as to + * expand the polytope. + * @param polytope The polytope contained in A ⊖ B. + * @param nearest_feature The feature containing the nearest point on the + * boundary of the polytope to the origin. + * @retval dir The support direction along which to expand the polytope. Notice + * that dir is a normalized vector. + * @throws UnexpectedConfigurationException if the nearest feature is a vertex. + */ +static ccd_vec3_t supportEPADirection(const ccd_pt_t* polytope, + const ccd_pt_el_t* nearest_feature) { + /* + If we denote the nearest point as v, when v is not the origin, then the + support direction is v. If v is the origin, then v should be an interior + point on a face, and the support direction is the normal of that face, + pointing outward from the polytope. + */ + ccd_vec3_t dir; + if (ccdIsZero(nearest_feature->dist)) { + // nearest point is the origin. + switch (nearest_feature->type) { + case CCD_PT_VERTEX: { + FCL_THROW_FAILED_AT_THIS_CONFIGURATION( + "The nearest point to the origin is a vertex of the polytope. This " + "should be identified as a touching contact"); + break; + } + case CCD_PT_EDGE: { + // When the nearest point is on an edge, the origin must be on that + // edge. The support direction could be in the range between + // edge.faces[0].normal and edge.faces[1].normal, where the face normals + // point outward from the polytope. In this implementation, we + // arbitrarily choose faces[0] normal. + const ccd_pt_edge_t* edge = + reinterpret_cast(nearest_feature); + dir = faceNormalPointingOutward(polytope, edge->faces[0]); + break; + } + case CCD_PT_FACE: { + // If origin is an interior point of a face, then choose the normal of + // that face as the sample direction. + const ccd_pt_face_t* face = + reinterpret_cast(nearest_feature); + dir = faceNormalPointingOutward(polytope, face); + break; + } } + } else { + ccdVec3Copy(&dir, &(nearest_feature->witness)); + } + ccdVec3Normalize(&dir); + return dir; +} - if (dist < ccd->epa_tolerance) - return -1; +/** Finds next support point (and stores it in out argument). + * @param[in] polytope The current polytope contained inside the Minkowski + * difference A ⊖ B. + * @param[in] obj1 Geometric object A. + * @param[in] obj2 Geometric object B. + * @param[in] ccd The libccd solver. + * @param[in] el The polytope boundary feature that contains the point nearest + * to the origin. + * @param[out] out The next support point. + * @retval status If the next support point is found, then returns 0; otherwise + * returns -1. There are several reasons why the next support point is not + * found: + * 1. If the nearest point on the boundary of the polytope to the origin is a + * vertex of the polytope. Then we know the two objects A and B are in touching + * contact. + * 2. If the difference between the upper bound and lower bound of the distance + * is below ccd->epa_tolerance, then we converge to a distance whose + * difference from the real distance is less than ccd->epa_tolerance. + */ +static int nextSupport(const ccd_pt_t* polytope, const void* obj1, + const void* obj2, const ccd_t* ccd, + const ccd_pt_el_t* el, ccd_support_t* out) { + ccd_vec3_t *a, *b, *c; + + if (el->type == CCD_PT_VERTEX) return -1; + + const ccd_vec3_t dir = supportEPADirection(polytope, el); + + __ccdSupport(obj1, obj2, &dir, ccd, out); + + // Compute distance of support point in the support direction, so we can + // determine whether we expanded a polytope surrounding the origin a bit. + const ccd_real_t dist = ccdVec3Dot(&out->v, &dir); + + // el->dist is the squared distance from the feature "el" to the origin.. + // dist is an upper bound on the distance from the boundary of the Minkowski + // difference to the origin, and sqrt(el->dist) is a lower bound of that + // distance. + if (dist - std::sqrt(el->dist) < ccd->epa_tolerance) return -1; + + ccd_real_t dist_squared{}; + if (el->type == CCD_PT_EDGE) { + // fetch end points of edge + ccdPtEdgeVec3(reinterpret_cast(el), &a, &b); + + // get distance from segment + dist_squared = ccdVec3PointSegmentDist2(&out->v, a, b, NULL); + } else { // el->type == CCD_PT_FACE + // fetch vertices of triangle face + ccdPtFaceVec3(reinterpret_cast(el), &a, &b, &c); + + // check if new point can significantly expand polytope + ccd_vec3_t point_projection_on_triangle_unused; + dist_squared = ccdVec3PointTriDist2(&out->v, a, b, c, + &point_projection_on_triangle_unused); + } - return 0; + if (std::sqrt(dist_squared) < ccd->epa_tolerance) return -1; + + return 0; } @@ -958,6 +1579,81 @@ static int __ccdGJK(const void *obj1, const void *obj2, return -1; } +/** + * When the nearest feature of a polytope to the origin is an edge, and the + * origin is inside the polytope, it implies one of the following conditions + * 1. The origin lies exactly on that edge + * 2. The two neighbouring faces of that edge are coplanar, and the projection + * of the origin onto that plane is on the edge. + * At times libccd incorrectly claims that the nearest feature is an edge. + * Inside this function, we will verify if one of these two conditions are true. + * If not, we will modify the nearest feature stored inside @p polytope, such + * that it stores the correct nearest feature and distance. + * @note we assume that even if the edge is not the correct nearest feature, the + * correct one should be one of the neighbouring faces of that edge. Namely the + * libccd solution is just slightly wrong. + * @param polytope The polytope whose nearest feature is being verified (and + * corrected if the edge should not be nearest feature). + * @note Only call this function in the EPA functions, where the origin should + * be inside the polytope. + */ +static void validateNearestFeatureOfPolytopeBeingEdge(ccd_pt_t* polytope) { + assert(polytope->nearest_type == CCD_PT_EDGE); + // Only verify the feature if the nearest feature is an edge. + + const ccd_pt_edge_t* const nearest_edge = + reinterpret_cast(polytope->nearest); + // Find the outward normals on the two neighbouring faces of the edge, if + // the origin is on the "inner" side of these two faces, then we regard the + // origin to be inside the polytope. Note that these face_normals are + // normalized. + std::array face_normals; + std::array origin_to_face_distance; + for (int i = 0; i < 2; ++i) { + face_normals[i] = + faceNormalPointingOutward(polytope, nearest_edge->faces[i]); + ccdVec3Normalize(&face_normals[i]); + // If the origin o is on the "inner" side of the face, then + // n̂ ⋅ (o - vₑ) ≤ 0 or, with simplification, -n̂ ⋅ vₑ ≤ 0 (since n̂ ⋅ o = 0). + origin_to_face_distance[i] = + -ccdVec3Dot(&face_normals[i], &nearest_edge->vertex[0]->v.v); + if (origin_to_face_distance[i] > 0) { + FCL_THROW_FAILED_AT_THIS_CONFIGURATION( + "The origin is outside of the polytope. This should already have " + "been identified as separating."); + } + } + // We compute the projection of the origin onto the plane as + // -face_normals[i] * origin_to_face_distance[i] + // If the projection to both faces are on the edge, the the edge is the + // closest feature. + bool is_edge_closest_feature = true; + for (int i = 0; i < 2; ++i) { + ccd_vec3_t origin_projection_to_plane = face_normals[i]; + ccdVec3Scale(&(origin_projection_to_plane), -origin_to_face_distance[i]); + if (!is_point_on_line_segment(origin_projection_to_plane, + nearest_edge->vertex[0]->v.v, + nearest_edge->vertex[1]->v.v)) { + is_edge_closest_feature = false; + break; + } + } + if (!is_edge_closest_feature) { + // We assume that libccd is not crazily wrong. Although the closest + // feature is not the edge, it is near that edge. Hence we select the + // neighboring face that is closest to the origin. + polytope->nearest_type = CCD_PT_FACE; + // Note origin_to_face_distance is the *signed* distance and it is + // guaranteed to be negative if we are here, hence sense of this + // comparison is reversed. + const int closest_face = + origin_to_face_distance[0] < origin_to_face_distance[1] ? 1 : 0; + polytope->nearest = + reinterpret_cast(nearest_edge->faces[closest_face]); + // polytope->nearest_dist stores the SQUARED distance. + polytope->nearest_dist = pow(origin_to_face_distance[closest_face], 2); + } +} static int __ccdEPA(const void *obj1, const void *obj2, const ccd_t *ccd, @@ -975,12 +1671,14 @@ static int __ccdEPA(const void *obj1, const void *obj2, size = ccdSimplexSize(simplex); if (size == 4){ ret = simplexToPolytope4(obj1, obj2, ccd, simplex, polytope, nearest); - }else if (size == 3){ - ret = simplexToPolytope3(obj1, obj2, ccd, simplex, polytope, nearest); + } else if (size == 3) { + ret = convert2SimplexToTetrahedron(obj1, obj2, ccd, simplex, polytope, + nearest); }else{ // size == 2 ret = simplexToPolytope2(obj1, obj2, ccd, simplex, polytope, nearest); } + if (ret == -1){ // touching contact return 0; @@ -990,12 +1688,21 @@ static int __ccdEPA(const void *obj1, const void *obj2, } while (1){ - // get triangle nearest to origin + // get triangle nearest to origin + *nearest = ccdPtNearest(polytope); + if (polytope->nearest_type == CCD_PT_EDGE) { + // When libccd thinks the nearest feature is an edge, that is often + // wrong, hence we validate the nearest feature by ourselves. + // TODO remove this validation step when we can reliably compute the + // nearest feature of a polytope. + validateNearestFeatureOfPolytopeBeingEdge(polytope); *nearest = ccdPtNearest(polytope); + } // get next support point - if (nextSupport(obj1, obj2, ccd, *nearest, &supp) != 0) + if (nextSupport(polytope, obj1, obj2, ccd, *nearest, &supp) != 0) { break; + } // expand nearest triangle using new point - supp if (expandPolytope(polytope, *nearest, &supp) != 0) @@ -1005,148 +1712,219 @@ static int __ccdEPA(const void *obj1, const void *obj2, return 0; } -/** Returns the points p1 and p2 on the original shapes that correspond to point - * p in the given simplex.*/ -static void extractClosestPoints(ccd_simplex_t* simplex, - ccd_vec3_t* p1, ccd_vec3_t* p2, ccd_vec3_t* p) -{ +/** Given a single support point, `q`, extract the point `p1` and `p2`, the + points on object 1 and 2, respectively, in the support data of `q`. */ +static void extractObjectPointsFromPoint(ccd_support_t *q, ccd_vec3_t *p1, + ccd_vec3_t *p2) { + // TODO(SeanCurtis-TRI): Determine if I should be demanding that p1 and p2 + // are defined. + // Closest points are the ones stored in the simplex + if (p1) *p1 = q->v1; + if (p2) *p2 = q->v2; +} + +/** Given two support points which define a line segment (`a` and `b`), and a + point on that line segment `p`, computes the points `p1` and `p2`, the points + on object 1 and 2, respectively, in the support data which correspond to `p`. + @pre `p = a + s(b - a), 0 <= s <= 1` */ +static void extractObjectPointsFromSegment(ccd_support_t *a, ccd_support_t *b, + ccd_vec3_t *p1, ccd_vec3_t *p2, + ccd_vec3_t *p) { + // Closest points lie on the segment defined by the points in the simplex + // Let the segment be defined by points A and B. We can write p as + // + // p = A + s*AB, 0 <= s <= 1 + // p - A = s*AB + ccd_vec3_t AB; + ccdVec3Sub2(&AB, &(b->v), &(a->v)); + + // This defines three equations, but we only need one. Taking the i-th + // component gives + // + // p_i - A_i = s*AB_i. + // + // Thus, s is given by + // + // s = (p_i - A_i)/AB_i. + // + // To avoid dividing by an AB_i ≪ 1, we choose i such that |AB_i| is + // maximized + ccd_real_t abs_AB_x{std::abs(ccdVec3X(&AB))}; + ccd_real_t abs_AB_y{std::abs(ccdVec3Y(&AB))}; + ccd_real_t abs_AB_z{std::abs(ccdVec3Z(&AB))}; + + ccd_real_t A_i, AB_i, p_i; + if (abs_AB_x >= abs_AB_y && abs_AB_x >= abs_AB_z) { + A_i = ccdVec3X(&(a->v)); + AB_i = ccdVec3X(&AB); + p_i = ccdVec3X(p); + } else if (abs_AB_y >= abs_AB_z) { + A_i = ccdVec3Y(&(a->v)); + AB_i = ccdVec3Y(&AB); + p_i = ccdVec3Y(p); + } else { + A_i = ccdVec3Z(&(a->v)); + AB_i = ccdVec3Z(&AB); + p_i = ccdVec3Z(p); + } + + if (std::abs(AB_i) < constants::eps()) { + // Points are coincident; treat as a single point. + extractObjectPointsFromPoint(a, p1, p2); + return; + } + + auto calc_p = [](ccd_vec3_t *p_a, ccd_vec3_t *p_b, ccd_vec3_t *p, + ccd_real_t s) { + ccd_vec3_t sAB; + ccdVec3Sub2(&sAB, p_b, p_a); + ccdVec3Scale(&sAB, s); + ccdVec3Copy(p, p_a); + ccdVec3Add(p, &sAB); + }; + + // TODO(SeanCurtis-TRI): If p1 or p2 is null, there seems little point in + // calling this method. It seems that both of these being non-null should be + // a *requirement*. Determine that this is the case and do so. + ccd_real_t s = (p_i - A_i) / AB_i; + + if (p1) { + // p1 = A1 + s*A1B1 + calc_p(&(a->v1), &(b->v1), p1, s); + } + if (p2) { + // p2 = A2 + s*A2B2 + calc_p(&(a->v2), &(b->v2), p2, s); + } +} + +/** Returns the points `p1` and `p2` on the original shapes that correspond to + point `p` in the given simplex. + @pre simplex size <= 3. + @pre p lies _on_ the simplex (i.e., within the triangle, line segment, or is + coincident with the point). */ +static void extractClosestPoints(ccd_simplex_t *simplex, ccd_vec3_t *p1, + ccd_vec3_t *p2, ccd_vec3_t *p) { const int simplex_size = ccdSimplexSize(simplex); assert(simplex_size <= 3); if (simplex_size == 1) { - // Closest points are the ones stored in the simplex - if(p1) *p1 = simplex->ps[simplex->last].v1; - if(p2) *p2 = simplex->ps[simplex->last].v2; + extractObjectPointsFromPoint(&simplex->ps[0], p1, p2); } - else if (simplex_size ==2) + else if (simplex_size == 2) + { + extractObjectPointsFromSegment(&simplex->ps[0], &simplex->ps[1], p1, p2, p); + } + else // simplex_size == 3 { - // Closest points lie on the segment defined by the points in the simplex - // Let the segment be defined by points A and B. We can write p as - // - // p = A + s*AB, 0 <= s <= 1 - // p - A = s*AB - ccd_vec3_t AB; + if (triangle_area_is_zero(simplex->ps[0].v, simplex->ps[1].v, + simplex->ps[2].v)) { + // The triangle is degenerate; compute the nearest point to a line + // segment. The segment is defined by the most-distant vertex pair. + int a_index, b_index; + ccd_vec3_t AB, AC, BC; ccdVec3Sub2(&AB, &(simplex->ps[1].v), &(simplex->ps[0].v)); - - // This defines three equations, but we only need one. Taking the i-th - // component gives - // - // p_i - A_i = s*AB_i. - // - // Thus, s is given by - // - // s = (p_i - A_i)/AB_i. - // - // To avoid dividing by an AB_i ≪ 1, we choose i such that |AB_i| is - // maximized - ccd_real_t abs_AB_x{std::abs(ccdVec3X(&AB))}; - ccd_real_t abs_AB_y{std::abs(ccdVec3Y(&AB))}; - ccd_real_t abs_AB_z{std::abs(ccdVec3Z(&AB))}; - ccd_real_t s{0}; - - if (abs_AB_x >= abs_AB_y && abs_AB_x >= abs_AB_z) { - ccd_real_t A_x{ccdVec3X(&(simplex->ps[0].v))}; - ccd_real_t AB_x{ccdVec3X(&AB)}; - ccd_real_t p_x{ccdVec3X(p)}; - s = (p_x - A_x) / AB_x; - } else if (abs_AB_y >= abs_AB_z) { - ccd_real_t A_y{ccdVec3Y(&(simplex->ps[0].v))}; - ccd_real_t AB_y{ccdVec3Y(&AB)}; - ccd_real_t p_y{ccdVec3Y(p)}; - s = (p_y - A_y) / AB_y; + ccdVec3Sub2(&AC, &(simplex->ps[2].v), &(simplex->ps[0].v)); + ccdVec3Sub2(&BC, &(simplex->ps[2].v), &(simplex->ps[1].v)); + ccd_real_t AB_len2 = ccdVec3Len2(&AB); + ccd_real_t AC_len2 = ccdVec3Len2(&AC); + ccd_real_t BC_len2 = ccdVec3Len2(&BC); + if (AB_len2 >= AC_len2 && AB_len2 >= BC_len2) { + a_index = 0; + b_index = 1; + } else if (AC_len2 >= AB_len2 && AC_len2 >= BC_len2) { + a_index = 0; + b_index = 2; } else { - ccd_real_t A_z{ccdVec3Z(&(simplex->ps[0].v))}; - ccd_real_t AB_z{ccdVec3Z(&AB)}; - ccd_real_t p_z{ccdVec3Z(p)}; - s = (p_z - A_z) / AB_z; + a_index = 1; + b_index = 2; } + extractObjectPointsFromSegment(&simplex->ps[a_index], + &simplex->ps[b_index], p1, p2, p); + return; + } - if (p1) - { - // p1 = A1 + s*A1B1 - ccd_vec3_t sAB; - ccdVec3Sub2(&sAB, &(simplex->ps[1].v1), &(simplex->ps[0].v1)); - ccdVec3Scale(&sAB, s); - ccdVec3Copy(p1, &(simplex->ps[0].v1)); - ccdVec3Add(p1, &sAB); - } - if (p2) - { - // p2 = A2 + s*A2B2 - ccd_vec3_t sAB; - ccdVec3Sub2(&sAB, &(simplex->ps[1].v2), &(simplex->ps[0].v2)); - ccdVec3Scale(&sAB, s); - ccdVec3Copy(p2, &(simplex->ps[0].v2)); - ccdVec3Add(p2, &sAB); - } - } - else // simplex_size == 3 - { - // Closest points lie in the triangle defined by the points in the simplex - ccd_vec3_t AB, AC, n, v_prime, AB_cross_v_prime, AC_cross_v_prime; - // Let the triangle be defined by points A, B, and C. The triangle lies in - // the plane that passes through A, whose normal is given by n̂, where + // Compute the barycentric coordinates of point p in triangle ABC. // - // n = AB × AC - // n̂ = n / ‖n‖ - ccdVec3Sub2(&AB, &(simplex->ps[1].v), &(simplex->ps[0].v)); - ccdVec3Sub2(&AC, &(simplex->ps[2].v), &(simplex->ps[0].v)); - ccdVec3Cross(&n, &AB, &AC); - - // Since p lies in ABC, it can be expressed as + // A + // ╱╲ p = αA + βB + γC + // ╱ |╲ + // ╱ | ╲ α = 1 - β - γ + // ╱ p | ╲ β = AREA(pAC) / AREA(ABC) + // ╱ / \ ╲ γ = AREA(pAB) / AREA(ABC) + // ╱__/_____\_╲ + // B C AREA(XYZ) = |r_XY × r_XZ| / 2 // - // p = A + p' - // p' = s*AB + t*AC + // Rewrite coordinates in terms of cross products. // - // where 0 <= s, t, s+t <= 1. - ccdVec3Sub2(&v_prime, p, &(simplex->ps[0].v)); - - // To find the corresponding v1 and v2, we need - // values for s and t. Taking cross products with AB and AC gives the - // following system: + // β = AREA(pAC) / AREA(ABC) = |r_Ap × r_AC| / |r_AB × r_AC| + // γ = AREA(pAB) / AREA(ABC) = |r_AB × r_Ap| / |r_AB × r_AC| // - // AB × p' = t*(AB × AC) = t*n - // AC × p' = -s*(AB × AC) = -s*n - ccdVec3Cross(&AB_cross_v_prime, &AB, &v_prime); - ccdVec3Cross(&AC_cross_v_prime, &AC, &v_prime); - - // To convert this to a system of scalar equations, we take the dot product - // with n: + // NOTE: There are multiple options for the cross products, these have been + // selected to re-use as many symbols as possible. // - // n ⋅ (AB × p') = t * ‖n‖² - // n ⋅ (AC × p') = -s * ‖n‖² - ccd_real_t norm_squared_n{ccdVec3Len2(&n)}; - - // Therefore, s and t are given by + // Solving for β and γ: + // + // β = |r_Ap × r_AC| / |r_AB × r_AC| + // β = |r_Ap × r_AC| / |n| n ≙ r_AB × r_AC, n̂ = n / |n| + // β = n̂·(r_Ap × r_AC) / n̂·n This step arises from the fact + // that (r_Ap × r_AC) and n point + // in the same direction. It + // allows us to take a single sqrt + // instead of three. + // β = (n/|n|)·(r_Ap × r_AC) / (n/|n|)·n + // β = n·(r_Ap × r_AC) / n·n + // β = n·(r_Ap × r_AC) / |n|² // - // s = -n ⋅ (AC × p') / ‖n‖² - // t = n ⋅ (AB × p') / ‖n‖² - ccd_real_t s{-ccdVec3Dot(&n, &AC_cross_v_prime) / norm_squared_n}; - ccd_real_t t{ccdVec3Dot(&n, &AB_cross_v_prime) / norm_squared_n}; + // A similar process to solve for gamma + // γ = n·(r_AB × r_Ap) / |n|² + + // Compute n and |n|². + ccd_vec3_t r_AB, r_AC, n; + ccdVec3Sub2(&r_AB, &(simplex->ps[1].v), &(simplex->ps[0].v)); + ccdVec3Sub2(&r_AC, &(simplex->ps[2].v), &(simplex->ps[0].v)); + ccdVec3Cross(&n, &r_AB, &r_AC); + ccd_real_t norm_squared_n{ccdVec3Len2(&n)}; - if (p1) - { - // p1 = A1 + s*A1B1 + t*A1C1 - ccd_vec3_t sAB, tAC; - ccdVec3Sub2(&sAB, &(simplex->ps[1].v1), &(simplex->ps[0].v1)); - ccdVec3Scale(&sAB, s); - ccdVec3Sub2(&tAC, &(simplex->ps[2].v1), &(simplex->ps[0].v1)); - ccdVec3Scale(&tAC, t); - ccdVec3Copy(p1, &(simplex->ps[0].v1)); - ccdVec3Add(p1, &sAB); - ccdVec3Add(p1, &tAC); + // Compute r_Ap. + ccd_vec3_t r_Ap; + ccdVec3Sub2(&r_Ap, p, &(simplex->ps[0].v)); + + // Compute the cross products in the numerators. + ccd_vec3_t r_Ap_cross_r_AC, r_AB_cross_r_Ap; + ccdVec3Cross(&r_Ap_cross_r_AC, &r_Ap, &r_AC); // r_Ap × r_AC + ccdVec3Cross(&r_AB_cross_r_Ap, &r_AB, &r_Ap); // r_AB × r_Ap + + // Compute beta and gamma. + ccd_real_t beta{ccdVec3Dot(&n, &r_Ap_cross_r_AC) / norm_squared_n}; + ccd_real_t gamma{ccdVec3Dot(&n, &r_AB_cross_r_Ap) / norm_squared_n}; + + // Evaluate barycentric interpolation (with the locally defined barycentric + // coordinates). + auto interpolate = [&beta, &gamma](const ccd_vec3_t& r_WA, + const ccd_vec3_t& r_WB, + const ccd_vec3_t& r_WC, + ccd_vec3_t* r_WP) { + // r_WP = r_WA + β * r_AB + γ * r_AC + ccdVec3Copy(r_WP, &r_WA); + + ccd_vec3_t beta_r_AB; + ccdVec3Sub2(&beta_r_AB, &r_WB, &r_WA); + ccdVec3Scale(&beta_r_AB, beta); + ccdVec3Add(r_WP, &beta_r_AB); + + ccd_vec3_t gamma_r_AC; + ccdVec3Sub2(&gamma_r_AC, &r_WC, &r_WA); + ccdVec3Scale(&gamma_r_AC, gamma); + ccdVec3Add(r_WP, &gamma_r_AC); + }; + + if (p1) { + interpolate(simplex->ps[0].v1, simplex->ps[1].v1, simplex->ps[2].v1, p1); } - if (p2) - { - // p2 = A2 + s*A2B2 + t*A2C2 - ccd_vec3_t sAB, tAC; - ccdVec3Sub2(&sAB, &(simplex->ps[1].v2), &(simplex->ps[0].v2)); - ccdVec3Scale(&sAB, s); - ccdVec3Sub2(&tAC, &(simplex->ps[2].v2), &(simplex->ps[0].v2)); - ccdVec3Scale(&tAC, t); - ccdVec3Copy(p2, &(simplex->ps[0].v2)); - ccdVec3Add(p2, &sAB); - ccdVec3Add(p2, &tAC); + + if (p2) { + interpolate(simplex->ps[0].v2, simplex->ps[1].v2, simplex->ps[2].v2, p2); } } } @@ -1197,7 +1975,7 @@ static inline ccd_real_t _ccdDist(const void *obj1, const void *obj2, &closest_p); dist = CCD_SQRT(dist); } - else if(ccdSimplexSize(simplex) == 3) + else if (ccdSimplexSize(simplex) == 3) { dist = ccdVec3PointTriDist2(ccd_vec3_origin, &ccdSimplexPoint(simplex, 0)->v, @@ -1249,55 +2027,77 @@ static inline ccd_real_t _ccdDist(const void *obj1, const void *obj2, return -CCD_REAL(1.); } -static int penEPAPosCmp(const void *a, const void *b) -{ - ccd_pt_vertex_t *v1, *v2; - v1 = *(ccd_pt_vertex_t **)a; - v2 = *(ccd_pt_vertex_t **)b; - - if (ccdEq(v1->dist, v2->dist)){ - return 0; - }else if (v1->dist < v2->dist){ - return -1; - }else{ - return 1; - } -} - -static int penEPAPosClosest(const ccd_pt_t *pt, const ccd_pt_el_t *nearest, - ccd_vec3_t *p1, ccd_vec3_t* p2) -{ - FCL_UNUSED(nearest); - - ccd_pt_vertex_t *v; - ccd_pt_vertex_t **vs; - size_t i, len; - // compute median - len = 0; - ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){ - len++; - } - - vs = CCD_ALLOC_ARR(ccd_pt_vertex_t*, len); - if (vs == NULL) - return -1; - - i = 0; - ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){ - vs[i++] = v; +/** + * Given the nearest point on the polytope inside the Minkowski difference + * A ⊖ B, returns the point p1 on geometric object A and p2 on geometric object + * B, such that p1 is the deepest penetration point on A, and p2 is the deepest + * penetration point on B. + * @param[in] nearest The feature with the point that is nearest to the origin + * on the boundary of the polytope. + * @param[out] p1 the deepest penetration point on A, measured and expressed in + * the world frame. + * @param[out] p2 the deepest penetration point on B, measured and expressed in + * the world frame. + * @retval status Return 0 on success, and -1 on failure. + */ +static int penEPAPosClosest(const ccd_pt_el_t* nearest, ccd_vec3_t* p1, + ccd_vec3_t* p2) { + // We reconstruct the simplex on which the nearest point lives, and then + // compute the deepest penetration point on each geometric objects. Note that + // the reconstructed simplex has size up to 3 (at most 3 vertices). + if (nearest->type == CCD_PT_VERTEX) { + ccd_pt_vertex_t* v = (ccd_pt_vertex_t*)nearest; + ccdVec3Copy(p1, &v->v.v1); + ccdVec3Copy(p2, &v->v.v2); + return 0; + } else { + ccd_simplex_t s; + ccdSimplexInit(&s); + if (nearest->type == CCD_PT_EDGE) { + const ccd_pt_edge_t* e = reinterpret_cast(nearest); + ccdSimplexAdd(&s, &(e->vertex[0]->v)); + ccdSimplexAdd(&s, &(e->vertex[1]->v)); + } else if (nearest->type == CCD_PT_FACE) { + const ccd_pt_face_t* f = reinterpret_cast(nearest); + // The face triangle has three edges, each edge consists of two end + // points, so there are 6 end points in total, each vertex of the triangle + // appears twice among the 6 end points. We need to choose the three + // distinctive vertices out of these 6 end points. + // First we pick edge0, the two end points of edge0 are distinct. + ccdSimplexAdd(&s, &(f->edge[0]->vertex[0]->v)); + ccdSimplexAdd(&s, &(f->edge[0]->vertex[1]->v)); + // Next we pick edge1, one of the two end points on edge1 is distinct from + // the end points in edge0, we will add this distinct vertex to the + // simplex. + for (int i = 0; i < 2; ++i) { + ccd_pt_vertex_t* third_vertex = f->edge[1]->vertex[i]; + if (third_vertex != f->edge[0]->vertex[0] && + third_vertex != f->edge[0]->vertex[1]) { + ccdSimplexAdd(&s, &(third_vertex->v)); + break; + } + } + } else { + throw std::logic_error( + "FCL penEPAPosClosest(): Unsupported feature type. The closest point " + "should be either a vertex, on an edge, or on a face."); } - - qsort(vs, len, sizeof(ccd_pt_vertex_t*), penEPAPosCmp); - - ccdVec3Copy(p1, &vs[0]->v.v1); - ccdVec3Copy(p2, &vs[0]->v.v2); - - free(vs); - + // Now compute the closest point in the simplex. + // TODO(hongkai.dai@tri.global): we do not need to compute the closest point + // on the simplex, as that is already given in `nearest`. We only need to + // extract the deepest penetration points on each geometric object. + // Sean.Curtis@tri.global and I will refactor this code in the future, to + // avoid calling extractClosestPoints. + ccd_vec3_t p; + ccdVec3Copy(&p, &(nearest->witness)); + extractClosestPoints(&s, p1, p2, &p); return 0; + } } -static inline ccd_real_t ccdGJKSignedDist(const void* obj1, const void* obj2, const ccd_t* ccd, ccd_vec3_t* p1, ccd_vec3_t* p2) +static inline ccd_real_t ccdGJKSignedDist(const void* obj1, const void* obj2, + const ccd_t* ccd, ccd_vec3_t* p1, + ccd_vec3_t* p2) { ccd_simplex_t simplex; @@ -1314,7 +2114,7 @@ static inline ccd_real_t ccdGJKSignedDist(const void* obj1, const void* obj2, co depth = -CCD_SQRT(nearest->dist); ccd_vec3_t pos1, pos2; - penEPAPosClosest(&polytope, nearest, &pos1, &pos2); + penEPAPosClosest(nearest, &pos1, &pos2); if (p1) *p1 = pos1; if (p2) *p2 = pos2; @@ -1358,7 +2158,9 @@ static inline ccd_real_t ccdGJKSignedDist(const void* obj1, const void* obj2, co // penetrating, -1 is returned. // @note Unlike _ccdDist function, this function does not need a warm-started // simplex as the input argument. -static inline ccd_real_t ccdGJKDist2(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_vec3_t* p1, ccd_vec3_t* p2) +static inline ccd_real_t ccdGJKDist2(const void *obj1, const void *obj2, + const ccd_t *ccd, ccd_vec3_t* p1, + ccd_vec3_t* p2) { ccd_simplex_t simplex; // first find an intersection @@ -1372,7 +2174,8 @@ static inline ccd_real_t ccdGJKDist2(const void *obj1, const void *obj2, const c /** Basic shape to ccd shape */ template -static void shapeToGJK(const ShapeBase& s, const Transform3& tf, ccd_obj_t* o) +static void shapeToGJK(const ShapeBase& s, const Transform3& tf, + ccd_obj_t* o) { FCL_UNUSED(s); @@ -1393,7 +2196,8 @@ static void boxToGJK(const Box& s, const Transform3& tf, ccd_box_t* box) } template -static void capToGJK(const Capsule& s, const Transform3& tf, ccd_cap_t* cap) +static void capToGJK(const Capsule& s, const Transform3& tf, + ccd_cap_t* cap) { shapeToGJK(s, tf, cap); cap->radius = s.radius; @@ -1401,7 +2205,8 @@ static void capToGJK(const Capsule& s, const Transform3& tf, ccd_cap_t* ca } template -static void cylToGJK(const Cylinder& s, const Transform3& tf, ccd_cyl_t* cyl) +static void cylToGJK(const Cylinder& s, const Transform3& tf, + ccd_cyl_t* cyl) { shapeToGJK(s, tf, cyl); cyl->radius = s.radius; @@ -1409,7 +2214,8 @@ static void cylToGJK(const Cylinder& s, const Transform3& tf, ccd_cyl_t* c } template -static void coneToGJK(const Cone& s, const Transform3& tf, ccd_cone_t* cone) +static void coneToGJK(const Cone& s, const Transform3& tf, + ccd_cone_t* cone) { shapeToGJK(s, tf, cone); cone->radius = s.radius; @@ -1417,14 +2223,16 @@ static void coneToGJK(const Cone& s, const Transform3& tf, ccd_cone_t* con } template -static void sphereToGJK(const Sphere& s, const Transform3& tf, ccd_sphere_t* sph) +static void sphereToGJK(const Sphere& s, const Transform3& tf, + ccd_sphere_t* sph) { shapeToGJK(s, tf, sph); sph->radius = s.radius; } template -static void ellipsoidToGJK(const Ellipsoid& s, const Transform3& tf, ccd_ellipsoid_t* ellipsoid) +static void ellipsoidToGJK(const Ellipsoid& s, const Transform3& tf, + ccd_ellipsoid_t* ellipsoid) { shapeToGJK(s, tf, ellipsoid); ellipsoid->radii[0] = s.radii[0]; @@ -1433,27 +2241,38 @@ static void ellipsoidToGJK(const Ellipsoid& s, const Transform3& tf, ccd_e } template -static void convexToGJK(const Convex& s, const Transform3& tf, ccd_convex_t* conv) +static void convexToGJK(const Convex& s, const Transform3& tf, + ccd_convex_t* conv) { shapeToGJK(s, tf, conv); conv->convex = &s; } /** Support functions */ -static inline void supportBox(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) -{ +static inline void supportBox(const void* obj, const ccd_vec3_t* dir_, + ccd_vec3_t* v) +{ + // Use a customized sign function, so that the support of the box always + // appears in one of the box vertices. + // Picking support vertices on the interior of faces/edges can lead to + // degenerate triangles in the EPA algorithm and are no more correct than just + // picking box corners. + auto sign = [](ccd_real_t x) -> ccd_real_t { + return x >= 0 ? ccd_real_t(1.0) : ccd_real_t(-1.0); + }; const ccd_box_t* o = static_cast(obj); ccd_vec3_t dir; ccdVec3Copy(&dir, dir_); ccdQuatRotVec(&dir, &o->rot_inv); - ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0], - ccdSign(ccdVec3Y(&dir)) * o->dim[1], - ccdSign(ccdVec3Z(&dir)) * o->dim[2]); + ccdVec3Set(v, sign(ccdVec3X(&dir)) * o->dim[0], + sign(ccdVec3Y(&dir)) * o->dim[1], + sign(ccdVec3Z(&dir)) * o->dim[2]); ccdQuatRotVec(v, &o->rot); ccdVec3Add(v, &o->pos); } -static inline void supportCap(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) +static inline void supportCap(const void* obj, const ccd_vec3_t* dir_, + ccd_vec3_t* v) { const ccd_cap_t* o = static_cast(obj); ccd_vec3_t dir, pos1, pos2; @@ -1470,7 +2289,7 @@ static inline void supportCap(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_ ccdVec3Add(&pos1, v); ccdVec3Add(&pos2, v); - if(ccdVec3Z (&dir) > 0) + if (ccdVec3Z (&dir) > 0) ccdVec3Copy(v, &pos1); else ccdVec3Copy(v, &pos2); @@ -1480,7 +2299,8 @@ static inline void supportCap(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_ ccdVec3Add(v, &o->pos); } -static inline void supportCyl(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) +static inline void supportCyl(const void* obj, const ccd_vec3_t* dir_, + ccd_vec3_t* v) { const ccd_cyl_t* cyl = static_cast(obj); ccd_vec3_t dir; @@ -1491,7 +2311,7 @@ static inline void supportCyl(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_ zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1]; zdist = sqrt(zdist); - if(ccdIsZero(zdist)) + if (ccdIsZero(zdist)) ccdVec3Set(v, 0., 0., ccdSign(ccdVec3Z(&dir)) * cyl->height); else { @@ -1507,7 +2327,8 @@ static inline void supportCyl(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_ ccdVec3Add(v, &cyl->pos); } -static inline void supportCone(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) +static inline void supportCone(const void* obj, const ccd_vec3_t* dir_, + ccd_vec3_t* v) { const ccd_cone_t* cone = static_cast(obj); ccd_vec3_t dir; @@ -1523,9 +2344,9 @@ static inline void supportCone(const void* obj, const ccd_vec3_t* dir_, ccd_vec3 double sin_a = cone->radius / sqrt(cone->radius * cone->radius + 4 * cone->height * cone->height); - if(dir.v[2] > len * sin_a) + if (dir.v[2] > len * sin_a) ccdVec3Set(v, 0., 0., cone->height); - else if(zdist > 0) + else if (zdist > 0) { rad = cone->radius / zdist; ccdVec3Set(v, rad * ccdVec3X(&dir), rad * ccdVec3Y(&dir), -cone->height); @@ -1538,7 +2359,8 @@ static inline void supportCone(const void* obj, const ccd_vec3_t* dir_, ccd_vec3 ccdVec3Add(v, &cone->pos); } -static inline void supportSphere(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) +static inline void supportSphere(const void* obj, const ccd_vec3_t* dir_, + ccd_vec3_t* v) { const ccd_sphere_t* s = static_cast(obj); ccd_vec3_t dir; @@ -1555,7 +2377,8 @@ static inline void supportSphere(const void* obj, const ccd_vec3_t* dir_, ccd_ve ccdVec3Add(v, &s->pos); } -static inline void supportEllipsoid(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) +static inline void supportEllipsoid(const void* obj, const ccd_vec3_t* dir_, + ccd_vec3_t* v) { const ccd_ellipsoid_t* s = static_cast(obj); ccd_vec3_t dir; @@ -1580,28 +2403,26 @@ static inline void supportEllipsoid(const void* obj, const ccd_vec3_t* dir_, ccd } template -static void supportConvex(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) +static void supportConvex(const void* obj, const ccd_vec3_t* dir_, + ccd_vec3_t* v) { const auto* c = (const ccd_convex_t*)obj; ccd_vec3_t dir, p; ccd_real_t maxdot, dot; - int i; - Vector3* curp; - const auto& center = c->convex->center; + const auto& center = c->convex->getInteriorPoint(); ccdVec3Copy(&dir, dir_); ccdQuatRotVec(&dir, &c->rot_inv); maxdot = -CCD_REAL_MAX; - curp = c->convex->points; - for(i = 0; i < c->convex->num_points; ++i, curp += 1) + for (const auto& vertex : c->convex->getVertices()) { - ccdVec3Set(&p, (*curp)[0] - center[0], (*curp)[1] - center[1], (*curp)[2] - center[2]); + ccdVec3Set(&p, vertex[0] - center[0], vertex[1] - center[1], + vertex[2] - center[2]); dot = ccdVec3Dot(&dir, &p); - if(dot > maxdot) - { - ccdVec3Set(v, (*curp)[0], (*curp)[1], (*curp)[2]); + if (dot > maxdot) { + ccdVec3Set(v, vertex[0], vertex[1], vertex[2]); maxdot = dot; } } @@ -1611,7 +2432,8 @@ static void supportConvex(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v ccdVec3Add(v, &c->pos); } -static void supportTriangle(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) +static void supportTriangle(const void* obj, const ccd_vec3_t* dir_, + ccd_vec3_t* v) { const ccd_triangle_t* tri = static_cast(obj); ccd_vec3_t dir, p; @@ -1623,11 +2445,12 @@ static void supportTriangle(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* maxdot = -CCD_REAL_MAX; - for(i = 0; i < 3; ++i) + for (i = 0; i < 3; ++i) { - ccdVec3Set(&p, tri->p[i].v[0] - tri->c.v[0], tri->p[i].v[1] - tri->c.v[1], tri->p[i].v[2] - tri->c.v[2]); + ccdVec3Set(&p, tri->p[i].v[0] - tri->c.v[0], tri->p[i].v[1] - tri->c.v[1], + tri->p[i].v[2] - tri->c.v[2]); dot = ccdVec3Dot(&dir, &p); - if(dot > maxdot) + if (dot > maxdot) { ccdVec3Copy(v, &tri->p[i]); maxdot = dot; @@ -1649,7 +2472,8 @@ template static void centerConvex(const void* obj, ccd_vec3_t* c) { const auto *o = static_cast*>(obj); - ccdVec3Set(c, o->convex->center[0], o->convex->center[1], o->convex->center[2]); + const Vector3& p = o->convex->getInteriorPoint(); + ccdVec3Set(c, p[0], p[1], p[2]); ccdQuatRotVec(c, &o->rot); ccdVec3Add(c, &o->pos); } @@ -1666,7 +2490,8 @@ template bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1, void* obj2, ccd_support_fn supp2, ccd_center_fn cen2, unsigned int max_iterations, S tolerance, - Vector3* contact_points, S* penetration_depth, Vector3* normal) + Vector3* contact_points, S* penetration_depth, + Vector3* normal) { ccd_t ccd; int res; @@ -1682,15 +2507,16 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1, ccd.max_iterations = max_iterations; ccd.mpr_tolerance = tolerance; - if(!contact_points) + if (!contact_points) { return ccdMPRIntersect(obj1, obj2, &ccd); } - /// libccd returns dir and pos in world space and dir is pointing from object 1 to object 2 + /// libccd returns dir and pos in world space and dir is pointing from + /// object 1 to object 2 res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos); - if(res == 0) + if (res == 0) { *contact_points << ccdVec3X(&pos), ccdVec3Y(&pos), ccdVec3Z(&pos); *penetration_depth = depth; @@ -1745,6 +2571,7 @@ bool GJKDistanceImpl(void* obj1, ccd_support_fn supp1, void* obj2, ccd.max_iterations = max_iterations; ccd.dist_tolerance = tolerance; + ccd.epa_tolerance = tolerance; ccd_vec3_t p1_, p2_; // NOTE(JS): p1_ and p2_ are set to zeros in order to suppress uninitialized @@ -1774,6 +2601,19 @@ bool GJKDistance(void* obj1, ccd_support_fn supp1, p1, p2); } +/** + * Compute the signed distance between two mesh objects. When the objects are + * separating, the signed distance is > 0. When the objects are touching or + * penetrating, the signed distance is <= 0. + * @param tolerance. When the objects are separating, the GJK algorithm + * terminates when the change of distance between iterations is smaller than + * this tolerance. Note that this does NOT necessarily mean that the computed + * distance is within @p tolerance to the actual distance. On the other hand, + * when the objects are penetrating, the EPA algorithm terminates when the + * difference between the upper bound and the lower bound of the penetration + * depth is smaller than @p tolerance. Hence the computed penetration depth is + * within @p tolerance to the true depth. + */ template bool GJKSignedDistance(void* obj1, ccd_support_fn supp1, void* obj2, ccd_support_fn supp2, @@ -1797,7 +2637,8 @@ GJKCenterFunction GJKInitializer>::getCenterFunction() } template -void* GJKInitializer>::createGJKObject(const Cylinder& s, const Transform3& tf) +void* GJKInitializer>::createGJKObject(const Cylinder& s, + const Transform3& tf) { ccd_cyl_t* o = new ccd_cyl_t; cylToGJK(s, tf, o); @@ -1824,7 +2665,8 @@ GJKCenterFunction GJKInitializer>::getCenterFunction() } template -void* GJKInitializer>::createGJKObject(const Sphere& s, const Transform3& tf) +void* GJKInitializer>::createGJKObject(const Sphere& s, + const Transform3& tf) { ccd_sphere_t* o = new ccd_sphere_t; sphereToGJK(s, tf, o); @@ -1851,7 +2693,8 @@ GJKCenterFunction GJKInitializer>::getCenterFunction() } template -void* GJKInitializer>::createGJKObject(const Ellipsoid& s, const Transform3& tf) +void* GJKInitializer>::createGJKObject(const Ellipsoid& s, + const Transform3& tf) { ccd_ellipsoid_t* o = new ccd_ellipsoid_t; ellipsoidToGJK(s, tf, o); @@ -1878,7 +2721,8 @@ GJKCenterFunction GJKInitializer>::getCenterFunction() } template -void* GJKInitializer>::createGJKObject(const Box& s, const Transform3& tf) +void* GJKInitializer>::createGJKObject(const Box& s, + const Transform3& tf) { ccd_box_t* o = new ccd_box_t; boxToGJK(s, tf, o); @@ -1905,7 +2749,8 @@ GJKCenterFunction GJKInitializer>::getCenterFunction() } template -void* GJKInitializer>::createGJKObject(const Capsule& s, const Transform3& tf) +void* GJKInitializer>::createGJKObject(const Capsule& s, + const Transform3& tf) { ccd_cap_t* o = new ccd_cap_t; capToGJK(s, tf, o); @@ -1932,7 +2777,8 @@ GJKCenterFunction GJKInitializer>::getCenterFunction() } template -void* GJKInitializer>::createGJKObject(const Cone& s, const Transform3& tf) +void* GJKInitializer>::createGJKObject(const Cone& s, + const Transform3& tf) { ccd_cone_t* o = new ccd_cone_t; coneToGJK(s, tf, o); @@ -1959,7 +2805,8 @@ GJKCenterFunction GJKInitializer>::getCenterFunction() } template -void* GJKInitializer>::createGJKObject(const Convex& s, const Transform3& tf) +void* GJKInitializer>::createGJKObject(const Convex& s, + const Transform3& tf) { auto* o = new ccd_convex_t; convexToGJK(s, tf, o); @@ -1984,10 +2831,12 @@ inline GJKCenterFunction triGetCenterFunction() } template -void* triCreateGJKObject(const Vector3& P1, const Vector3& P2, const Vector3& P3) +void* triCreateGJKObject(const Vector3& P1, const Vector3& P2, + const Vector3& P3) { ccd_triangle_t* o = new ccd_triangle_t; - Vector3 center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3); + Vector3 center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, + (P1[2] + P2[2] + P3[2]) / 3); ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]); ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]); @@ -2001,10 +2850,12 @@ void* triCreateGJKObject(const Vector3& P1, const Vector3& P2, const Vecto } template -void* triCreateGJKObject(const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf) +void* triCreateGJKObject(const Vector3& P1, const Vector3& P2, + const Vector3& P3, const Transform3& tf) { ccd_triangle_t* o = new ccd_triangle_t; - Vector3 center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3); + Vector3 center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, + (P1[2] + P2[2] + P3[2]) / 3); ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]); ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]); diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h index f639f4bc3..9f1309e06 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h @@ -183,14 +183,13 @@ Vector3 getSupport( { const Convex* convex = static_cast*>(shape); S maxdot = - std::numeric_limits::max(); - Vector3* curp = convex->points; Vector3 bestv = Vector3::Zero(); - for(int i = 0; i < convex->num_points; ++i, curp+=1) + for(const auto& vertex : convex->getVertices()) { - S dot = dir.dot(*curp); + S dot = dir.dot(vertex); if(dot > maxdot) { - bestv = *curp; + bestv = vertex; maxdot = dot; } } diff --git a/include/fcl/narrowphase/detail/failed_at_this_configuration.h b/include/fcl/narrowphase/detail/failed_at_this_configuration.h new file mode 100644 index 000000000..b36bb5952 --- /dev/null +++ b/include/fcl/narrowphase/detail/failed_at_this_configuration.h @@ -0,0 +1,125 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019. 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 CNRS-LAAS and AIST 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) */ + +#ifndef FCL_FAILED_AT_THIS_CONFIGURATION_H +#define FCL_FAILED_AT_THIS_CONFIGURATION_H + +#include +#include +#include +#include +#include +#include + +#include "fcl/export.h" + +namespace fcl { +namespace detail { + +/** A custom exception type that can be thrown in low-level code indicating + that the program has reached an unexpected configuration and that the caller + that ultimately has access to the collision objects and their transforms + should report the configuration that led to the exception. + + This is strictly an _internal_ exception; it should *always* be caught and + transformed into an exception that propagates to the operating system. + + Recommended usage is to throw by invoking the macro + FCL_THROW_UNEXPECTED_CONFIGURATION_EXCEPTION defined below. Code that exercises + functionality that throws this type of exception should catch it and transform + it to a `std::logic_error` by invoking ThrowDetailedConfiguration(). */ +class FCL_EXPORT FailedAtThisConfiguration final + : public std::exception { + public: + FailedAtThisConfiguration(const std::string& message) + : std::exception(), message_(message) {} + + const char* what() const noexcept final { return message_.c_str(); } + + private: + std::string message_; +}; + +/** Helper function for dispatching an `FailedAtThisConfiguration`. + Because this exception is designed to be caught and repackaged, we lose the + automatic association with file and line number. This wraps them into the + message of the exception so it can be preserved in the re-wrapping. + + @param message The error message itself. + @param func The name of the invoking function. + @param file The name of the file associated with the exception. + @param line The line number where the exception is thrown. */ +FCL_EXPORT void ThrowFailedAtThisConfiguration( + const std::string& message, const char* func, const char* file, int line); + +/** Helper class for propagating a low-level exception upwards but with + configuration-specific details appended. The parameters + + @param s1 The first shape in the query. + @param X_FS1 The pose of the first shape in frame F. + @param s2 The second shape in the query. + @param X_FS2 The pose of the second shape in frame F. + @param solver The solver. + @param e The original exception. + @tparam Shape1 The type of shape for shape 1. + @tparam Shape2 The type of shape for shape 2. + @tparam Solver The solver type (with scalar type erase). + @tparam Pose The pose type (a Transform with scalar type erased). + */ +template +void ThrowDetailedConfiguration(const Shape1& s1, const Pose& X_FS1, + const Shape2& s2, const Pose& X_FS2, + const Solver& solver, const std::exception& e) { + std::stringstream ss; + ss << std::setprecision(20); + ss << "Error with configuration" + << "\n Original error message: " << e.what() + << "\n Shape 1: " << s1 + << "\n X_FS1\n" << X_FS1.matrix() + << "\n Shape 2: " << s2 + << "\n X_FS2\n" << X_FS2.matrix() + << "\n Solver: " << solver; + throw std::logic_error(ss.str()); +} + +} // namespace detail +} // namespace fcl + +#define FCL_THROW_FAILED_AT_THIS_CONFIGURATION(message) \ + ::fcl::detail::ThrowFailedAtThisConfiguration(message, __func__, __FILE__, \ + __LINE__) + +#endif // FCL_FAILED_AT_THIS_CONFIGURATION_H diff --git a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h index e75236202..ef9f8b158 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h @@ -49,12 +49,15 @@ #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk.h" #include "fcl/narrowphase/detail/convexity_based_algorithm/epa.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h" +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h" +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/plane.h" +#include "fcl/narrowphase/detail/failed_at_this_configuration.h" namespace fcl { @@ -181,9 +184,9 @@ bool GJKSolver_indep::shapeIntersect( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | box | O | | | | | | O | O | | +// | box | O | O | | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | | O | | | O | O | O | +// | sphere |/////| O | | O | | O | O | O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -246,6 +249,10 @@ FCL_GJK_INDEP_SHAPE_INTERSECT(Box, detail::boxBoxIntersect) FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleIntersect) +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect) + +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Cylinder, detail::sphereCylinderIntersect) + FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, detail::sphereHalfspaceIntersect) FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, detail::ellipsoidHalfspaceIntersect) FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceIntersect) @@ -619,8 +626,10 @@ struct ShapeDistanceIndepImpl if(distance) *distance = (w0 - w1).norm(); - if(p1) *p1 = w0; - if(p2) (*p2).noalias() = shape.toshape0.inverse() * w1; + // Answer is solved in Shape1's local frame; answers are given in the + // world frame. + if(p1) p1->noalias() = tf1 * w0; + if(p2) p2->noalias() = tf1 * w1; return true; } @@ -661,8 +670,14 @@ bool GJKSolver_indep::shapeSignedDistance( Vector3* p2) const { // TODO: should implement the signed distance version - return ShapeDistanceIndepImpl::run( + bool result = false; + try { + result = ShapeDistanceIndepImpl::run( *this, s1, tf1, s2, tf2, dist, p1, p2); + } catch (const FailedAtThisConfiguration& e) { + ThrowDetailedConfiguration(s1, tf1, s2, tf2, *this, e); + } + return result; } // Shape distance algorithms not using built-in GJK algorithm @@ -670,9 +685,9 @@ bool GJKSolver_indep::shapeSignedDistance( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | box | | | | | | | | | | +// | box | | O | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | | O | | | | | O | +// | sphere |/////| O | | O | | O | | | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -689,6 +704,42 @@ bool GJKSolver_indep::shapeSignedDistance( // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +//============================================================================== +template +struct ShapeDistanceIndepImpl, Box> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Sphere& s1, + const Transform3& tf1, + const Box& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereBoxDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeDistanceIndepImpl, Sphere> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Box& s1, + const Transform3& tf1, + const Sphere& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereBoxDistance(s2, tf2, s1, tf1, dist, p2, p1); + } +}; + //============================================================================== template struct ShapeDistanceIndepImpl, Capsule> @@ -725,6 +776,42 @@ struct ShapeDistanceIndepImpl, Sphere> } }; +//============================================================================== +template +struct ShapeDistanceIndepImpl, Cylinder> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Sphere& s1, + const Transform3& tf1, + const Cylinder& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereCylinderDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeDistanceIndepImpl, Sphere> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Cylinder& s1, + const Transform3& tf1, + const Sphere& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereCylinderDistance(s2, tf2, s1, tf1, dist, p2, p1); + } +}; + //============================================================================== template struct ShapeDistanceIndepImpl, Sphere> @@ -802,8 +889,9 @@ struct ShapeTriangleDistanceIndepImpl } if(distance) *distance = (w0 - w1).norm(); + // The answers are produced in world coordinates. Keep them there. if(p1) *p1 = w0; - if(p2) (*p2).noalias() = shape.toshape0 * w1; + if(p2) *p2 = w1; return true; } else @@ -892,8 +980,8 @@ struct ShapeTransformedTriangleDistanceIndepImpl } if(distance) *distance = (w0 - w1).norm(); - if(p1) *p1 = w0; - if(p2) (*p2).noalias() = shape.toshape0 * w1; + if(p1) p1->noalias() = tf1 * w0; + if(p2) p2->noalias() = tf1 * w1; return true; } else diff --git a/include/fcl/narrowphase/detail/gjk_solver_indep.h b/include/fcl/narrowphase/detail/gjk_solver_indep.h index 1d08e3fa7..14eb10ada 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_indep.h +++ b/include/fcl/narrowphase/detail/gjk_solver_indep.h @@ -38,6 +38,8 @@ #ifndef FCL_NARROWPHASE_GJKSOLVERINDEP_H #define FCL_NARROWPHASE_GJKSOLVERINDEP_H +#include + #include "fcl/common/types.h" #include "fcl/narrowphase/contact_point.h" @@ -179,6 +181,20 @@ struct FCL_EXPORT GJKSolver_indep /// @brief smart guess mutable Vector3 cached_guess; + + friend + std::ostream& operator<<(std::ostream& out, const GJKSolver_indep& solver) { + out << "GjkSolver_indep" + << "\n gjk tolerance: " << solver.gjk_tolerance + << "\n gjk max iterations: " << solver.gjk_max_iterations + << "\n epa tolerance: " << solver.epa_tolerance + << "\n epa max face num: " << solver.epa_max_face_num + << "\n epa max vertex num: " << solver.epa_max_vertex_num + << "\n epa max iterations: " << solver.epa_max_iterations + << "\n enable cahced guess: " << solver.enable_cached_guess; + if (solver.enable_cached_guess) out << solver.cached_guess.transpose(); + return out; + } }; using GJKSolver_indepf = GJKSolver_indep; diff --git a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h index 7f0a60b82..3cef4d657 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h @@ -46,12 +46,15 @@ #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h" +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h" +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/plane.h" +#include "fcl/narrowphase/detail/failed_at_this_configuration.h" namespace fcl { @@ -177,9 +180,9 @@ bool GJKSolver_libccd::shapeIntersect( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | box | O | | | | | | O | O | | +// | box | O | O | | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | | O | | | O | O | O | +// | sphere |/////| O | | O | | O | O | O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| | | | | O | O | TODO | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -242,6 +245,10 @@ FCL_GJK_LIBCCD_SHAPE_INTERSECT(Box, detail::boxBoxIntersect) FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleIntersect) +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect) + +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Cylinder, detail::sphereCylinderIntersect) + FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, detail::sphereHalfspaceIntersect) FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, detail::ellipsoidHalfspaceIntersect) FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceIntersect) @@ -550,7 +557,7 @@ struct ShapeSignedDistanceLibccdImpl void* o1 = detail::GJKInitializer::createGJKObject(s1, tf1); void* o2 = detail::GJKInitializer::createGJKObject(s2, tf2); - bool res = detail::GJKSignedDistance( + bool res = detail::GJKSignedDistance( o1, detail::GJKInitializer::getSupportFunction(), o2, @@ -561,12 +568,6 @@ struct ShapeSignedDistanceLibccdImpl p1, p2); - if (p1) - (*p1).noalias() = tf1.inverse(Eigen::Isometry) * *p1; - - if (p2) - (*p2).noalias() = tf2.inverse(Eigen::Isometry) * *p2; - detail::GJKInitializer::deleteGJKObject(o1); detail::GJKInitializer::deleteGJKObject(o2); @@ -585,8 +586,14 @@ bool GJKSolver_libccd::shapeSignedDistance( Vector3* p1, Vector3* p2) const { - return ShapeSignedDistanceLibccdImpl::run( + bool result = false; + try { + result = ShapeSignedDistanceLibccdImpl::run( *this, s1, tf1, s2, tf2, dist, p1, p2); + } catch (const FailedAtThisConfiguration& e) { + ThrowDetailedConfiguration(s1, tf1, s2, tf2, *this, e); + } + return result; } @@ -618,12 +625,6 @@ struct ShapeDistanceLibccdImpl p1, p2); - if (p1) - (*p1).noalias() = tf1.inverse(Eigen::Isometry) * *p1; - - if (p2) - (*p2).noalias() = tf2.inverse(Eigen::Isometry) * *p2; - detail::GJKInitializer::deleteGJKObject(o1); detail::GJKInitializer::deleteGJKObject(o2); @@ -651,9 +652,9 @@ bool GJKSolver_libccd::shapeDistance( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | box | | | | | | | | | | +// | box | | O | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | | O | | | | | O | +// | sphere |/////| O | | O | | O | | | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -670,6 +671,42 @@ bool GJKSolver_libccd::shapeDistance( // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +//============================================================================== +template +struct ShapeDistanceLibccdImpl, Box> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Sphere& s1, + const Transform3& tf1, + const Box& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereBoxDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeDistanceLibccdImpl, Sphere> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Box& s1, + const Transform3& tf1, + const Sphere& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereBoxDistance(s2, tf2, s1, tf1, dist, p2, p1); + } +}; + //============================================================================== template struct ShapeDistanceLibccdImpl, Capsule> @@ -706,6 +743,42 @@ struct ShapeDistanceLibccdImpl, Sphere> } }; +//============================================================================== +template +struct ShapeDistanceLibccdImpl, Cylinder> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Sphere& s1, + const Transform3& tf1, + const Cylinder& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereCylinderDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeDistanceLibccdImpl, Sphere> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Cylinder& s1, + const Transform3& tf1, + const Sphere& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereCylinderDistance(s2, tf2, s1, tf1, dist, p2, p1); + } +}; + //============================================================================== template struct ShapeDistanceLibccdImpl, Sphere> @@ -770,8 +843,6 @@ struct ShapeTriangleDistanceLibccdImpl dist, p1, p2); - if(p1) - (*p1).noalias() = tf.inverse(Eigen::Isometry) * *p1; detail::GJKInitializer::deleteGJKObject(o1); detail::triDeleteGJKObject(o2); @@ -845,10 +916,6 @@ struct ShapeTransformedTriangleDistanceLibccdImpl dist, p1, p2); - if(p1) - (*p1).noalias() = tf1.inverse(Eigen::Isometry) * *p1; - if(p2) - (*p2).noalias() = tf2.inverse(Eigen::Isometry) * *p2; detail::GJKInitializer::deleteGJKObject(o1); detail::triDeleteGJKObject(o2); diff --git a/include/fcl/narrowphase/detail/gjk_solver_libccd.h b/include/fcl/narrowphase/detail/gjk_solver_libccd.h index 2676427da..218a89481 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_libccd.h +++ b/include/fcl/narrowphase/detail/gjk_solver_libccd.h @@ -38,6 +38,8 @@ #ifndef FCL_NARROWPHASE_GJKSOLVERLIBCCD_H #define FCL_NARROWPHASE_GJKSOLVERLIBCCD_H +#include + #include "fcl/common/types.h" #include "fcl/narrowphase/contact_point.h" @@ -168,6 +170,17 @@ struct FCL_EXPORT GJKSolver_libccd /// @brief the threshold used in GJK algorithm to stop distance iteration S distance_tolerance; + friend + std::ostream& operator<<(std::ostream& out, const GJKSolver_libccd& solver) { + out << "GjkSolver_libccd" + << "\n collision_tolerance: " << solver.collision_tolerance + << "\n max collision iterations: " << solver.max_collision_iterations + << "\n distance tolerance: " << solver.distance_tolerance + << "\n max distance iterations: " << solver.max_distance_iterations; + // NOTE: Cached guesses are not supported. + return out; + } + }; using GJKSolver_libccdf = GJKSolver_libccd; diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h index 28a32b8be..91ffeca65 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h @@ -393,7 +393,7 @@ int boxBox2( // For small boxes (dimensions all less than 1), limit the scale factor to // be no smaller than 10 * eps. This assumes all dimensions are strictly // non-negative. - S scale_factor = max(max(A.maxCoeff(), B.maxCoeff()), 1.0) * 10 * eps; + S scale_factor = max(max(A.maxCoeff(), B.maxCoeff()), S(1.0)) * S(10) * eps; Q.array() += scale_factor; } diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h index e89330df4..70a9a872d 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h @@ -500,9 +500,9 @@ bool convexHalfspaceIntersect(const Convex& s1, const Transform3& tf1, Vector3 v; S depth = std::numeric_limits::max(); - for(int i = 0; i < s1.num_points; ++i) + for (const auto& vertex : s1.getVertices()) { - Vector3 p = tf1 * s1.points[i]; + Vector3 p = tf1 * vertex; S d = new_s2.signedDistance(p); if(d < depth) diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h index 858a8608f..1373dce1f 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h @@ -648,9 +648,9 @@ bool convexPlaneIntersect(const Convex& s1, const Transform3& tf1, Vector3 v_min, v_max; S d_min = std::numeric_limits::max(), d_max = -std::numeric_limits::max(); - for(int i = 0; i < s1.num_points; ++i) + for (const auto& vertex : s1.getVertices()) { - Vector3 p = tf1 * s1.points[i]; + Vector3 p = tf1 * vertex; S d = new_s2.signedDistance(p); diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h new file mode 100644 index 000000000..53cdace4e --- /dev/null +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h @@ -0,0 +1,231 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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 Sean Curtis (2018) */ + +#ifndef FCL_NARROWPHASE_DETAIL_SPHEREBOX_INL_H +#define FCL_NARROWPHASE_DETAIL_SPHEREBOX_INL_H + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h" + +namespace fcl { +namespace detail { + +extern template FCL_EXPORT bool +sphereBoxIntersect(const Sphere& sphere, const Transform3& X_FS, + const Box& box, const Transform3& X_FB, + std::vector>* contacts); + +//============================================================================== + +extern template FCL_EXPORT bool +sphereBoxDistance(const Sphere& sphere, const Transform3& X_FS, + const Box& box, const Transform3& X_FB, + double* distance, Vector3* p_FSb, + Vector3* p_FBs); + +//============================================================================== + +// Helper function for box-sphere queries. Given a box defined in its canonical +// frame B (i.e., aligned to the axes and centered on the origin) and a query +// point Q, determines point N, the point *inside* the box nearest to Q. Note: +// this is *not* the nearest point on the surface of the box; if Q is inside +// the box, then the nearest point is Q itself. +// @param size The size of the box to query against. +// @param p_BQ The position vector from frame B's origin to the query +// point Q measured and expressed in frame B. +// @param[out] p_BN_ptr A position vector from frame B's origin to the point N +// measured and expressed in frame B. +// @returns true if the nearest point is a different point than the query point. +// @pre P_BN_ptr must point to a valid Vector3 instance. +template +bool nearestPointInBox(const Vector3& size, const Vector3& p_BQ, + Vector3* p_BN_ptr) { + assert(p_BN_ptr != nullptr); + Vector3& p_BN = *p_BN_ptr; + // Clamp the point to the box. If we do *any* clamping we know the center was + // outside. If we did *no* clamping, the point is inside the box. + const Vector3 half_size = size / 2; + // The nearest point on the box (N) to Q measured in frame B. + bool clamped = false; + for (int i = 0; i < 3; ++i) { + p_BN(i) = p_BQ(i); + if (p_BQ(i) < -half_size(i)) { + clamped = true; + p_BN(i) = -half_size(i); + } + if (p_BQ(i) > half_size(i)) { + clamped = true; + p_BN(i) = half_size(i); + } + } + return clamped; +} +//============================================================================== + +template +FCL_EXPORT bool sphereBoxIntersect(const Sphere& sphere, + const Transform3& X_FS, const Box& box, + const Transform3& X_FB, + std::vector>* contacts) { + const S r = sphere.radius; + // Find the sphere center C in the box's frame. + const Transform3 X_BS = X_FB.inverse() * X_FS; + const Vector3 p_BC = X_BS.translation(); + + // Find N, the nearest point *inside* the box to the sphere center C (measured + // and expressed in frame B) + Vector3 p_BN; + bool N_is_not_C = nearestPointInBox(box.side, p_BC, &p_BN); + + // Compute the position vector from the nearest point N to the sphere center + // C in the common box frame B. If the center is inside the box, this will be + // the zero vector. + Vector3 p_CN_B = p_BN - p_BC; + S squared_distance = p_CN_B.squaredNorm(); + // The nearest point to the sphere is *farther* than radius, they are *not* + // penetrating. + if (squared_distance > r * r) + return false; + + // Now we know they are colliding. + + if (contacts != nullptr) { + // Return values have been requested. + S depth{0}; + Vector3 n_SB_B; // Normal pointing from sphere into box (in box's frame) + Vector3 p_BP; // Contact position (P) in the box frame. + // We want to make sure that differences exceed machine precision -- we + // don't want normal and contact position dominated by noise. However, + // because we apply an arbitrary rigid transform to the sphere's center, we + // lose bits of precision. For an arbitrary non-identity transform, 4 bits + // is the maximum possible precision loss. So, we only consider a point to + // be outside the box if it's distance is at least that epsilon. + // Furthermore, in finding the *near* face, a better candidate must be more + // than this epsilon closer to the sphere center (see the test in the + // else branch). + auto eps = 16 * constants::eps(); + if (N_is_not_C && squared_distance > eps * eps) { + // The center is on the outside. Normal direction is from C to N (computed + // above) and penetration depth is r - |p_BN - p_BC|. The contact position + // is 1/2 the penetration distance in the normal direction from p_BN. + S distance = sqrt(squared_distance); + n_SB_B = p_CN_B / distance; + depth = r - distance; + p_BP = p_BN + n_SB_B * (depth * 0.5); + } else { + // The center is inside. The sphere center projects onto all faces. The + // face that is closest defines the normal direction. The penetration + // depth is the distance to that face + radius. The position is the point + // midway between the projection point, and the point opposite the sphere + // center in the *negative* normal direction. + Vector3 half_size = box.side / 2; + S min_distance = + std::numeric_limits::Real>::infinity(); + int min_axis = -1; + for (int i = 0; i < 3; ++i) { + S dist = p_BC(i) >= 0 ? half_size(i) - p_BC(i) : p_BC(i) + half_size(i); + // To be closer, the face has to be more than epsilon closer. + if (dist + eps < min_distance) { + min_distance = dist; + min_axis = i; + } + } + n_SB_B << 0, 0, 0; + // NOTE: This sign *may* seem counter-intuitive. A center nearest the +z + // face produces a normal in the -z direction; this is because the normal + // points from the sphere and into the box; and the penetration is *into* + // the +z face (so points in the -z direction). The same logic applies to + // all other directions. + n_SB_B(min_axis) = p_BC(min_axis) >= 0 ? -1 : 1; + depth = min_distance + r; + p_BP = p_BC + n_SB_B * ((r - min_distance) / 2); + } + contacts->emplace_back(X_FB.linear() * n_SB_B, X_FB * p_BP, depth); + } + return true; +} + +//============================================================================== + +template +FCL_EXPORT bool sphereBoxDistance(const Sphere& sphere, + const Transform3& X_FS, const Box& box, + const Transform3& X_FB, S* distance, + Vector3* p_FSb, Vector3* p_FBs) { + // Find the sphere center C in the box's frame. + const Transform3 X_BS = X_FB.inverse() * X_FS; + const Vector3 p_BC = X_BS.translation(); + const S r = sphere.radius; + + // Find N, the nearest point *inside* the box to the sphere center C (measured + // and expressed in frame B) + Vector3 p_BN; + bool N_is_not_C = nearestPointInBox(box.side, p_BC, &p_BN); + + if (N_is_not_C) { + // If N is not C, we know the sphere center is *outside* the box (but we + // don't know yet if the they are completely separated). + + // Compute the position vector from the nearest point N to the sphere center + // C in the frame B. + Vector3 p_NC_B = p_BC - p_BN; + S squared_distance = p_NC_B.squaredNorm(); + if (squared_distance > r * r) { + // The distance to the nearest point is greater than the radius, we have + // proven separation. + S d{-1}; + if (distance || p_FBs || p_FSb) + d = sqrt(squared_distance); + if (distance != nullptr) + *distance = d - r; + if (p_FBs != nullptr) + *p_FBs = X_FB * p_BN; + if (p_FSb != nullptr) { + const Vector3 p_BSb = (p_NC_B / d) * (d - r) + p_BN; + *p_FSb = X_FB * p_BSb; + } + return true; + } + } + + // We didn't *prove* separation, so we must be in penetration. + if (distance != nullptr) *distance = -1; + return false; +} + +} // namespace detail +} // namespace fcl + +#endif // FCL_NARROWPHASE_DETAIL_SPHEREBOX_INL_H diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h new file mode 100644 index 000000000..87fabf8cd --- /dev/null +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h @@ -0,0 +1,142 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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 Sean Curtis (2018) */ + +#ifndef FCL_NARROWPHASE_DETAIL_SPHEREBOX_H +#define FCL_NARROWPHASE_DETAIL_SPHEREBOX_H + +#include "fcl/geometry/shape/box.h" +#include "fcl/geometry/shape/sphere.h" +#include "fcl/narrowphase/contact_point.h" + +namespace fcl { + +namespace detail { + +/** @name Custom box-sphere proximity algorithms + + These functions provide custom algorithms for analyzing the relationship + between a sphere and a box. + + These functions make use of the + [Drake monogram + notation](http://drake.mit.edu/doxygen_cxx/group__multibody__notation__basics.html) + to describe quantities (particularly the poses of shapes). + + Both shapes must be posed in a common frame (notated as F). This common frame + is typically the world frame W. Regardless, if the optional output data is + returned, it will be reported in this common frame F. + + The functions can be executed in one of two ways: to perform a strict boolean + query (is colliding/is separated) or to get data which characterizes the + colliding or separating state (e.g., penetration depth vs separating distance). + The difference in usage is defined by whether the optional out parameters + are null or non-null. In the documentation, these are labeled "(optional)". + + For these functions, if the sphere and box are detected to be *touching* this + is considered a collision. As such, a collision query would report true and + a separating distance query would report false. + */ + +// NOTE: the choice to consider touching contact as collision is predicated on +// the implementation in sphere-sphere contact. + +//@{ + +// NOTE: The handling of the discontinuities in normal and position was +// implicitly defined in the spherebox test in test_fcl_geometric_shapes.cpp. It +// provided backwards compatibility to that test. + +/** Detect collision between the sphere and box. If colliding, return + characterization of collision in the provided vector. + + The reported depth is guaranteed to be continuous with respect to the relative + pose of the two objects. The normal and contact position are guaranteed to be + continuous while the sphere center lies *outside* the box. However, if the + sphere center lies *inside* the box, there are regions of discontinuity in both + normal and contact position. This is due to the fact that there is not + necessarily a *unique* characterization of penetration depth (i.e., the sphere + center may be equidistant to multiple faces). In this case, the faces are + arbitrarily prioritized and the face with the highest priority is used to + compute normal and position. The priority order is +x, -x, +y, -y, +z, and -z. + For example: + + - If the center is near the edge between the -y and +z faces, the normal will + be defined w.r.t. to the -y face. + - If the center is near the corner of the +x, +y, & +z faces, the +x face + will be used. + + @param sphere The sphere geometry. + @param X_FS The pose of the sphere S in the common frame F. + @param box The box geometry. + @param X_FB The pose of the box B in the common frame F. + @param contacts[out] (optional) If the shapes collide, the contact point data + will be appended to the end of this vector. + @return True if the objects are colliding (including touching). + @tparam S The scalar parameter (must be a valid Eigen scalar). */ +template +FCL_EXPORT bool sphereBoxIntersect(const Sphere& sphere, + const Transform3& X_FS, const Box& box, + const Transform3& X_FB, + std::vector>* contacts); + +/** Evaluate the minimum separating distance between a sphere and box. If + separated, the nearest points on each shape will be returned in frame F. + @param sphere The sphere geometry. + @param X_FS The pose of the sphere S in the common frame F. + @param box The box geometry. + @param X_FB The pose of the box B in the common frame F. + @param distance[out] (optional) The separating distance between the box + and sphere. Set to -1 if the shapes are penetrating. + @param p_FSb[out] (optional) The closest point on the *sphere* to the box + measured and expressed in frame F. + @param p_FBs[out] (optional) The closest point on the *box* to the sphere + measured and expressed in frame F. + @return True if the objects are separated. + @tparam S The scalar parameter (must be a valid Eigen scalar). */ +template +FCL_EXPORT bool sphereBoxDistance(const Sphere& sphere, + const Transform3& X_FS, const Box& box, + const Transform3& X_FB, S* distance, + Vector3* p_FSb, Vector3* p_FBs); + +//@} + +} // namespace detail +} // namespace fcl + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h" + +#endif // FCL_NARROWPHASE_DETAIL_SPHEREBOX_H diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h index dd524ad6a..c85d56991 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h @@ -137,8 +137,16 @@ bool sphereCapsuleDistance(const Sphere& s1, const Transform3& tf1, S distance = diff.norm() - s1.radius - s2.radius; - if(distance <= 0) + if(distance <= 0) { + // NOTE: By assigning this a negative value, it allows the ultimately + // calling code in distance-inl.h (distance() method) to use collision to + // determine penetration depth and contact points. NOTE: This is a + // *horrible* thing. + // TODO(SeanCurtis-TRI): Create a *signed* distance variant of this and use + // it to determined signed distance, penetration, and distance. + if (dist) *dist = -1; return false; + } if(dist) *dist = distance; @@ -146,10 +154,14 @@ bool sphereCapsuleDistance(const Sphere& s1, const Transform3& tf1, if(p1) { *p1 = s_c - diff * s1.radius; - *p1 = tf1.inverse(Eigen::Isometry) * tf2 * (*p1); + *p1 = tf2 * (*p1); } - if(p2) *p2 = segment_point + diff * s1.radius; + if(p2) + { + *p2 = segment_point + diff * s2.radius; + *p2 = tf2 * (*p2); + } return true; } diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h new file mode 100644 index 000000000..44070c04e --- /dev/null +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h @@ -0,0 +1,276 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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 Sean Curtis (2018) */ + +#ifndef FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_INL_H +#define FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_INL_H + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h" + +namespace fcl { +namespace detail { + +extern template FCL_EXPORT bool +sphereCylinderIntersect(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + std::vector>* contacts); + +//============================================================================== + +extern template FCL_EXPORT bool +sphereCylinderDistance(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + double* distance, Vector3* p_FSc, + Vector3* p_FCs); + +//============================================================================== + +// Helper function for cylinder-sphere queries. Given a cylinder defined in its +// canonical frame C (i.e., centered on the origin with cylinder's height axis +// aligned to the Cz axis) and a query point Q, determines point N, the point +// *inside* the cylinder nearest to Q. Note: this is *not* necessarily the +// nearest point on the surface of the cylinder; if Q is inside the cylinder, +// then the nearest point is Q itself. +// @param height The height of the cylinder. +// @param radius The radius of the cylinder. +// @param p_CQ The position vector from frame C's origin to the query +// point Q measured and expressed in frame C. +// @param[out] p_CN_ptr A position vector from frame C's origin to the point N +// measured and expressed in frame C. +// @returns true if the nearest point is a different point than the query point. +// @pre p_CN_ptr must point to a valid Vector3 instance. +template +bool nearestPointInCylinder(const S& height, const S& radius, + const Vector3& p_CQ, Vector3* p_CN_ptr) { + assert(p_CN_ptr != nullptr); + Vector3& p_CN = *p_CN_ptr; + p_CN = p_CQ; + + bool clamped = false; + + // Linearly clamp along the cylinders height axis. + const S half_height = height / 2; + if (p_CQ(2) > half_height) { + clamped = true; + p_CN(2) = half_height; + } else if (p_CQ(2) < -half_height) { + clamped = true; + p_CN(2) = -half_height; + } + + // Clamp according to the circular cross section. + Vector2 r_CQ{p_CQ(0), p_CQ(1)}; + S squared_distance = r_CQ.dot(r_CQ); + if (squared_distance > radius * radius) { + // The query point lies outside the *circular* extent of the cylinder. + clamped = true; + r_CQ *= radius / sqrt(squared_distance); + p_CN(0) = r_CQ(0); + p_CN(1) = r_CQ(1); + } + + return clamped; +} + +//============================================================================== + +template +FCL_EXPORT bool sphereCylinderIntersect( + const Sphere& sphere, const Transform3& X_FS, + const Cylinder& cylinder, const Transform3& X_FC, + std::vector>* contacts) { + const S& r_s = sphere.radius; + // Find the sphere center So (abbreviated as S) in the cylinder's frame. + const Transform3 X_CS = X_FC.inverse() * X_FS; + const Vector3 p_CS = X_CS.translation(); + + // Find N, the nearest point *inside* the cylinder to the sphere center S + // (measure and expressed in frame C). + Vector3 p_CN; + bool S_is_outside = nearestPointInCylinder(cylinder.lz, cylinder.radius, p_CS, + &p_CN); + + // Compute the position vector from the sphere center S to the nearest point N + // in the cylinder frame C. If the center is inside the cylinder, this will + // be the zero vector. + const Vector3 p_SN_C = p_CN - p_CS; + const S p_SN_squared_dist = p_SN_C.squaredNorm(); + // The nearest point to the sphere is *farther* than radius; they are *not* + // penetrating. + if (p_SN_squared_dist > r_s * r_s) + return false; + + // Now we know they are colliding. + + if (contacts != nullptr) { + // Return values have been requested. + S depth{0}; + // Normal pointing from sphere into cylinder (in cylinder's frame) + Vector3 n_SC_C; + // Contact position (P) in the cylinder frame. + Vector3 p_CP; + + // We want to make sure that differences exceed machine precision -- we + // don't want normal and contact position dominated by noise. However, + // because we apply an arbitrary rigid transform to the sphere's center, we + // lose bits of precision. For an arbitrary non-identity transform, 4 bits + // is the maximum possible precision loss. So, we only consider a point to + // be outside the cylinder if it's distance is at least that epsilon. + // Furthermore, in finding the *near* face, a better candidate must be more + // than this epsilon closer to the sphere center (see the test in the + // else branch). + const auto eps = 16 * constants::eps(); + if (S_is_outside && p_SN_squared_dist > eps * eps) { + // The sphere center is *measurably outside* the cylinder. There are three + // possibilities: nearest point lies on the cap face, cap edge, or barrel. + // In all three cases, the normal points from the nearest point to the + // sphere center. Penetration depth is the radius minus the distance + // between the pair of points. And the contact point is simply half the + // depth from the nearest point in the normal direction. + + // Distance from closest point (N) to sphere center (S). + const S d_NS = sqrt(p_SN_squared_dist); + n_SC_C = p_SN_C / d_NS; + depth = r_s - d_NS; + p_CP = p_CN + n_SC_C * (depth * 0.5); + } else { + // The center is inside. It's either nearer the barrel or the end face + // (with preference for the end face). + const S& h = cylinder.lz; + const S face_distance = p_CS(2) >= 0 ? h / 2 - p_CS(2) : p_CS(2) + h / 2; + // For the barrel to be picked over the face, it must be more than + // epsilon closer to the sphere center. + + // The direction from the sphere to the nearest point on the barrel on + // the z = 0 plane. + const Vector2 n_SB_xy = Vector2(p_CS(0), p_CS(1)); + const S d_CS_xy = n_SB_xy.norm(); + const S barrel_distance = cylinder.radius - d_CS_xy; + // If the center is near the Voronoi boundary between the near face and + // the barrel, then this test would be affected by the precision loss + // inherent in computing p_CS. If we did a *strict* comparison, then + // we would get a different answer just by changing X_FC. This requires + // the barrel to be closer by an amount that subsumes the potential + // precision loss. + if (barrel_distance < face_distance - eps) { + // Closest to the barrel. + if (d_CS_xy > eps) { + // Normal towards barrel + n_SC_C << -n_SB_xy(0) / d_CS_xy, -n_SB_xy(1) / d_CS_xy, 0; + depth = r_s + barrel_distance; + p_CP = p_CS + n_SC_C * ((r_s - barrel_distance) / 2); + } else { + // Sphere center is on the central spine of the cylinder, as per + // documentation, assume we have penetration coming in from the +x + // direction. + n_SC_C = -Vector3::UnitX(); + depth = r_s + cylinder.radius; + p_CP = p_CS + n_SC_C * ((r_s - barrel_distance) / 2); + } + } else { + // Closest to the face. + n_SC_C << 0, 0, 0; + // NOTE: This sign *may* seem counter-intuitive. A center nearest the +z + // face produces a normal in the -z direction; this is because the + // normal points from the sphere and into the cylinder; and the + // penetration is *into* the +z face (so points in the -z direction). + n_SC_C(2) = p_CS(2) >= 0 ? -1 : 1; + depth = face_distance + r_s; + p_CP = p_CS + n_SC_C * ((r_s - face_distance) / 2); + } + } + contacts->emplace_back(X_FC.linear() * n_SC_C, X_FC * p_CP, depth); + } + return true; +} + +//============================================================================== + +template +FCL_EXPORT bool sphereCylinderDistance(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, S* distance, + Vector3* p_FSc, Vector3* p_FCs) { + // Find the sphere center S in the cylinder's frame. + const Transform3 X_CS = X_FC.inverse() * X_FS; + const Vector3 p_CS = X_CS.translation(); + const S r_s = sphere.radius; + + // Find N, the nearest point *inside* the cylinder to the sphere center S + // (measured and expressed in frame C). + Vector3 p_CN; + bool S_is_outside = nearestPointInCylinder(cylinder.lz, cylinder.radius, p_CS, + &p_CN); + + if (S_is_outside) { + // If N is not S, we know the sphere center is *outside* the cylinder (but + // we don't know yet if the they are completely separated). + + // Compute the position vector from the nearest point N to the sphere center + // S in the frame C. + const Vector3 p_NS_C = p_CS - p_CN; + const S p_NS_squared_dist = p_NS_C.squaredNorm(); + if (p_NS_squared_dist > r_s * r_s) { + // The distance to the nearest point is greater than the sphere radius; + // we have proven separation. + S d{-1}; + if (distance || p_FCs || p_FSc) + d = sqrt(p_NS_squared_dist); + if (distance != nullptr) + *distance = d - r_s; + if (p_FCs != nullptr) + *p_FCs = X_FC * p_CN; + if (p_FSc != nullptr) { + const Vector3 p_CSc = p_CS - (p_NS_C * r_s / d); + *p_FSc = X_FC * p_CSc; + } + return true; + } + } + + // We didn't *prove* separation, so we must be in penetration. + if (distance != nullptr) *distance = -1; + return false; +} + +} // namespace detail +} // namespace fcl + +#endif // FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_INL_H diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h new file mode 100644 index 000000000..4209b03a0 --- /dev/null +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h @@ -0,0 +1,139 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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 Sean Curtis (2018) */ + +#ifndef FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_H +#define FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_H + +#include "fcl/geometry/shape/cylinder.h" +#include "fcl/geometry/shape/sphere.h" +#include "fcl/narrowphase/contact_point.h" + +namespace fcl { + +namespace detail { + +/** @name Custom cylinder-sphere proximity algorithms + + These functions provide custom algorithms for analyzing the relationship + between a sphere and a cylinder. + + These functions make use of the + [Drake monogram + notation](http://drake.mit.edu/doxygen_cxx/group__multibody__notation__basics.html) + to describe quantities (particularly the poses of shapes). + + Both shapes must be posed in a common frame (notated as F). This common frame + is typically the world frame W. Regardless, if the optional output data is + returned, it will be reported in this common frame F. + + The functions can be executed in one of two ways: to perform a strict boolean + query (is colliding/is separated) or to get data which characterizes the + colliding or separating state (e.g., penetration depth vs separating distance). + The difference in usage is defined by whether the optional out parameters + are null or non-null. In the documentation, these are labeled "(optional)". + + For these functions, if the sphere and cylinder are detected to be *touching* + this is considered a collision. As such, a collision query would report true + and a separating distance query would report false. + */ + +// NOTE: the choice to consider touching contact as collision is predicated on +// the implementation in sphere-sphere contact. + +//@{ + +/** Detect collision between the sphere and cylinder. If colliding, return + characterization of collision in the provided vector. + + The reported depth is guaranteed to be continuous with respect to the relative + pose. In contrast, the normal and contact position are only guaranteed to be + similarly continuous while the sphere center lies *outside* the cylinder. + However, if the sphere center lies *inside* the cylinder, there are regions of + discontinuity in both normal and contact position. This is due to the fact that + there is not necessarily a *unique* characterization of penetration depth + (e.g., the sphere center may be equidistant to both a cap face as well as the + barrel). This ambiguity is resolved through an arbitrary prioritization scheme. + If the sphere center is equidistant to both an end face and the barrel, the end + face is used for normal and contact position computation. If the sphere center + is closer to the barrel, but there is no unique solution (i.e., the sphere + center lies on the center axis), the sphere is arbitrarily considered to be + penetrating from the direction of the cylinder's +x direction (yielding a + contact normal in the cylinder's -x direction.) + + @param sphere The sphere geometry. + @param X_FS The pose of the sphere S in the common frame F. + @param cylinder The cylinder geometry. + @param X_FC The pose of the cylinder C in the common frame F. + @param contacts[out] (optional) If the shapes collide, the contact point data + will be appended to the end of this vector. + @return True if the objects are colliding (including touching). + @tparam S The scalar parameter (must be a valid Eigen scalar). */ +template +FCL_EXPORT bool sphereCylinderIntersect(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + std::vector>* contacts); + +/** Evaluate the minimum separating distance between a sphere and cylinder. If + separated, the nearest points on each shape will be returned in frame F. + @param sphere The sphere geometry. + @param X_FS The pose of the sphere S in the common frame F. + @param cylinder The cylinder geometry. + @param X_FC The pose of the cylinder C in the common frame F. + @param distance[out] (optional) The separating distance between the cylinder + and sphere. Set to -1 if the shapes are penetrating. + @param p_FSc[out] (optional) The closest point on the *sphere* to the + cylinder measured and expressed in frame F. + @param p_FCs[out] (optional) The closest point on the *cylinder* to the + sphere measured and expressed in frame F. + @return True if the objects are separated. + @tparam S The scalar parameter (must be a valid Eigen scalar). */ +template +FCL_EXPORT bool sphereCylinderDistance(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, S* distance, + Vector3* p_FSc, Vector3* p_FCs); + +//@} + +} // namespace detail +} // namespace fcl + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h" + +#endif // FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_H diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h index efeeca4eb..d12671cfb 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h @@ -99,8 +99,8 @@ bool sphereSphereDistance(const Sphere& s1, const Transform3& tf1, if(len > s1.radius + s2.radius) { if(dist) *dist = len - (s1.radius + s2.radius); - if(p1) *p1 = tf1.inverse(Eigen::Isometry) * (o1 - diff * (s1.radius / len)); - if(p2) *p2 = tf2.inverse(Eigen::Isometry) * (o2 + diff * (s2.radius / len)); + if(p1) *p1 = (o1 - diff * (s1.radius / len)); + if(p2) *p2 = (o2 + diff * (s2.radius / len)); return true; } diff --git a/include/fcl/narrowphase/distance_result.h b/include/fcl/narrowphase/distance_result.h index fb53b36d8..045d2c80d 100644 --- a/include/fcl/narrowphase/distance_result.h +++ b/include/fcl/narrowphase/distance_result.h @@ -63,9 +63,9 @@ struct FCL_EXPORT DistanceResult /// @sa DistanceRequest::enable_signed_distance S min_distance; - /// @brief Nearest points in the world coordinates + /// @brief Nearest points in the world coordinates. /// - /// @sa DeistanceRequest::enable_nearest_points + /// @sa DistanceRequest::enable_nearest_points Vector3 nearest_points[2]; /// @brief collision object 1 diff --git a/src/math/bv/utility.cpp b/src/math/bv/utility.cpp index 66fc4aeba..d2e44a9c9 100644 --- a/src/math/bv/utility.cpp +++ b/src/math/bv/utility.cpp @@ -46,23 +46,23 @@ namespace OBB_fit_functions { //============================================================================== template -void fit1(Vector3* ps, OBB& bv); +void fit1(const Vector3d* const ps, OBB& bv); //============================================================================== template -void fit2(Vector3* ps, OBB& bv); +void fit2(const Vector3d* const ps, OBB& bv); //============================================================================== template -void fit3(Vector3* ps, OBB& bv); +void fit3(const Vector3d* const ps, OBB& bv); //============================================================================== template -void fit6(Vector3* ps, OBB& bv); +void fit6(const Vector3d* const ps, OBB& bv); //============================================================================== template -void fitn(Vector3* ps, int n, OBB& bv); +void fitn(const Vector3d* const ps, int n, OBB& bv); //============================================================================== } // namespace OBB_fit_functions @@ -74,23 +74,23 @@ namespace RSS_fit_functions { //============================================================================== template -void fit1(Vector3* ps, RSS& bv); +void fit1(const Vector3d* const ps, RSS& bv); //============================================================================== template -void fit2(Vector3* ps, RSS& bv); +void fit2(const Vector3d* const ps, RSS& bv); //============================================================================== template -void fit3(Vector3* ps, RSS& bv); +void fit3(const Vector3d* const ps, RSS& bv); //============================================================================== template -void fit6(Vector3* ps, RSS& bv); +void fit6(const Vector3d* const ps, RSS& bv); //============================================================================== template -void fitn(Vector3* ps, int n, RSS& bv); +void fitn(const Vector3d* const ps, int n, RSS& bv); //============================================================================== } // namespace RSS_fit_functions @@ -102,19 +102,19 @@ namespace kIOS_fit_functions { //============================================================================== template -void fit1(Vector3* ps, kIOS& bv); +void fit1(const Vector3d* const ps, kIOS& bv); //============================================================================== template -void fit2(Vector3* ps, kIOS& bv); +void fit2(const Vector3d* const ps, kIOS& bv); //============================================================================== template -void fit3(Vector3* ps, kIOS& bv); +void fit3(const Vector3d* const ps, kIOS& bv); //============================================================================== template -void fitn(Vector3* ps, int n, kIOS& bv); +void fitn(const Vector3d* const ps, int n, kIOS& bv); //============================================================================== } // namespace kIOS_fit_functions @@ -126,19 +126,19 @@ namespace OBBRSS_fit_functions { //============================================================================== template -void fit1(Vector3* ps, OBBRSS& bv); +void fit1(const Vector3d* const ps, OBBRSS& bv); //============================================================================== template -void fit2(Vector3* ps, OBBRSS& bv); +void fit2(const Vector3d* const ps, OBBRSS& bv); //============================================================================== template -void fit3(Vector3* ps, OBBRSS& bv); +void fit3(const Vector3d* const ps, OBBRSS& bv); //============================================================================== template -void fitn(Vector3* ps, int n, OBBRSS& bv); +void fitn(const Vector3d* const ps, int n, OBBRSS& bv); //============================================================================== } // namespace OBBRSS_fit_functions diff --git a/src/math/geometry.cpp b/src/math/geometry.cpp index 8304d4544..b40110f69 100644 --- a/src/math/geometry.cpp +++ b/src/math/geometry.cpp @@ -77,8 +77,8 @@ void generateCoordinateSystem(Transform3d& tf); //============================================================================== template void getRadiusAndOriginAndRectangleSize( - Vector3d* ps, - Vector3d* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -90,8 +90,8 @@ void getRadiusAndOriginAndRectangleSize( //============================================================================== template void getRadiusAndOriginAndRectangleSize( - Vector3d* ps, - Vector3d* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -111,8 +111,8 @@ void circumCircleComputation( //============================================================================== template double maximumDistance( - Vector3d* ps, - Vector3d* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -121,8 +121,8 @@ double maximumDistance( //============================================================================== template void getExtentAndCenter( - Vector3d* ps, - Vector3d* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, Triangle* ts, unsigned int* indices, int n, @@ -133,8 +133,8 @@ void getExtentAndCenter( //============================================================================== template void getCovariance( - Vector3d* ps, - Vector3d* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, Triangle* ts, unsigned int* indices, int n, Matrix3d& M); @@ -146,44 +146,44 @@ namespace detail { //============================================================================== template double maximumDistance_mesh( - Vector3* ps, - Vector3* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, Triangle* ts, unsigned int* indices, int n, - const Vector3& query); + const Vector3d& query); //============================================================================== template double maximumDistance_pointcloud( - Vector3* ps, - Vector3* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, unsigned int* indices, int n, - const Vector3& query); + const Vector3d& query); //============================================================================== template void getExtentAndCenter_pointcloud( - Vector3* ps, - Vector3* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, unsigned int* indices, int n, - const Matrix3& axis, - Vector3& center, - Vector3& extent); + const Matrix3d& axis, + Vector3d& center, + Vector3d& extent); //============================================================================== template void getExtentAndCenter_mesh( - Vector3* ps, - Vector3* ps2, + const Vector3d* const ps, + const Vector3d* const ps2, Triangle* ts, unsigned int* indices, int n, - const Matrix3& axis, - Vector3& center, - Vector3& extent); + const Matrix3d& axis, + Vector3d& center, + Vector3d& extent); //============================================================================== } // namespace detail diff --git a/src/narrowphase/detail/failed_at_this_configuration.cpp b/src/narrowphase/detail/failed_at_this_configuration.cpp new file mode 100644 index 000000000..ba7875d3e --- /dev/null +++ b/src/narrowphase/detail/failed_at_this_configuration.cpp @@ -0,0 +1,17 @@ +#include "fcl/narrowphase/detail/failed_at_this_configuration.h" + +#include + +namespace fcl { +namespace detail { + +void ThrowFailedAtThisConfiguration(const std::string& message, + const char* func, + const char* file, int line) { + std::stringstream ss; + ss << file << ":(" << line << "): " << func << "(): " << message; + throw FailedAtThisConfiguration(ss.str()); +} + +} // namespace detail +} // namespace fcl diff --git a/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp b/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp new file mode 100644 index 000000000..291ad3063 --- /dev/null +++ b/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp @@ -0,0 +1,60 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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 CNRS-LAAS and AIST 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) (2018) */ + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h" + +namespace fcl +{ + +namespace detail +{ + +//============================================================================== +template bool +sphereBoxIntersect(const Sphere& sphere, const Transform3& X_FS, + const Box& box, const Transform3& X_FB, + std::vector>* contacts); + +//============================================================================== + +template bool +sphereBoxDistance(const Sphere& sphere, const Transform3& X_FS, + const Box& box, const Transform3& X_FB, + double* distance, Vector3* p_FSb, + Vector3* p_FBs); + +} // namespace detail +} // namespace fcl diff --git a/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp b/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp new file mode 100644 index 000000000..d9e87d599 --- /dev/null +++ b/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp @@ -0,0 +1,63 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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 CNRS-LAAS and AIST 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) (2018) */ + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h" + +namespace fcl +{ + +namespace detail +{ + +template bool +sphereCylinderIntersect(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + std::vector>* contacts); + +//============================================================================== + +template bool +sphereCylinderDistance(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + double* distance, Vector3* p_FSc, + Vector3* p_FCs); + +} // namespace detail +} // namespace fcl diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index cc599b137..c2156d2f2 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -52,13 +52,16 @@ set(tests test_fcl_distance.cpp test_fcl_frontlist.cpp test_fcl_general.cpp + test_fcl_generate_bvh_model_deferred_finalize.cpp test_fcl_geometric_shapes.cpp test_fcl_math.cpp test_fcl_profiler.cpp test_fcl_shape_mesh_consistency.cpp test_fcl_signed_distance.cpp test_fcl_simple.cpp + test_fcl_sphere_box.cpp test_fcl_sphere_capsule.cpp + test_fcl_sphere_cylinder.cpp test_fcl_sphere_sphere.cpp ) @@ -94,4 +97,5 @@ foreach(test ${tests}) add_fcl_test(${test}) endforeach(test) +add_subdirectory(geometry) add_subdirectory(narrowphase) diff --git a/test/eigen_matrix_compare.h b/test/eigen_matrix_compare.h new file mode 100644 index 000000000..11e88474a --- /dev/null +++ b/test/eigen_matrix_compare.h @@ -0,0 +1,150 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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. + */ + +// This code was taken from Drake. +// https://github.com/RobotLocomotion/drake/blob/master/common/test_utilities/eigen_matrix_compare.h + +#ifndef FCL_EIGEN_MATRIX_COMPARE_H +#define FCL_EIGEN_MATRIX_COMPARE_H + +#include +#include +#include + +#include +#include + +namespace fcl { + +enum class MatrixCompareType { absolute, relative }; + +/** + * Compares two matrices to determine whether they are equal to within a certain + * threshold. + * + * @param m1 The first matrix to compare. + * @param m2 The second matrix to compare. + * @param tolerance The tolerance for determining equivalence. + * @param compare_type Whether the tolerance is absolute or relative. + * @return true if the two matrices are equal based on the specified tolerance. + */ +template +::testing::AssertionResult CompareMatrices( + const Eigen::MatrixBase& m1, + const Eigen::MatrixBase& m2, double tolerance = 0.0, + MatrixCompareType compare_type = MatrixCompareType::absolute) { + if (m1.rows() != m2.rows() || m1.cols() != m2.cols()) { + return ::testing::AssertionFailure() + << "Matrix size mismatch: (" << m1.rows() << " x " << m1.cols() + << " vs. " << m2.rows() << " x " << m2.cols() << ")"; + } + + for (int ii = 0; ii < m1.rows(); ii++) { + for (int jj = 0; jj < m1.cols(); jj++) { + // First handle the corner cases of positive infinity, negative infinity, + // and NaN + const auto both_positive_infinity = + m1(ii, jj) == std::numeric_limits::infinity() && + m2(ii, jj) == std::numeric_limits::infinity(); + + const auto both_negative_infinity = + m1(ii, jj) == -std::numeric_limits::infinity() && + m2(ii, jj) == -std::numeric_limits::infinity(); + + using std::isnan; + const auto both_nan = isnan(m1(ii, jj)) && isnan(m2(ii, jj)); + + if (both_positive_infinity || both_negative_infinity || both_nan) + continue; + + // Check for case where one value is NaN and the other is not + if ((isnan(m1(ii, jj)) && !isnan(m2(ii, jj))) || + (!isnan(m1(ii, jj)) && isnan(m2(ii, jj)))) { + return ::testing::AssertionFailure() << "NaN missmatch at (" << ii + << ", " << jj << "):\nm1 =\n" + << m1 << "\nm2 =\n" + << m2; + } + + // Determine whether the difference between the two matrices is less than + // the tolerance. + using std::abs; + const auto delta = abs(m1(ii, jj) - m2(ii, jj)); + + if (compare_type == MatrixCompareType::absolute) { + // Perform comparison using absolute tolerance. + + if (delta > tolerance) { + return ::testing::AssertionFailure() + << "Values at (" << ii << ", " << jj + << ") exceed tolerance: " << m1(ii, jj) << " vs. " + << m2(ii, jj) << ", diff = " << delta + << ", tolerance = " << tolerance << "\nm1 =\n" + << m1 << "\nm2 =\n" + << m2 << "\ndelta=\n" + << (m1 - m2); + } + } else { + // Perform comparison using relative tolerance, see: + // http://realtimecollisiondetection.net/blog/?p=89 + using std::max; + const auto max_value = max(abs(m1(ii, jj)), abs(m2(ii, jj))); + const auto relative_tolerance = + tolerance * max(decltype(max_value){1}, max_value); + + if (delta > relative_tolerance) { + return ::testing::AssertionFailure() + << "Values at (" << ii << ", " << jj + << ") exceed tolerance: " << m1(ii, jj) << " vs. " + << m2(ii, jj) << ", diff = " << delta + << ", tolerance = " << tolerance + << ", relative tolerance = " << relative_tolerance + << "\nm1 =\n" + << m1 << "\nm2 =\n" + << m2 << "\ndelta=\n" + << (m1 - m2); + } + } + } + } + + return ::testing::AssertionSuccess() << "m1 =\n" + << m1 + << "\nis approximately equal to m2 =\n" + << m2; +} + +} // namespace fcl + +#endif // FCL_EIGEN_MATRIX_COMPARE_H diff --git a/test/expect_throws_message.h b/test/expect_throws_message.h new file mode 100644 index 000000000..3932492f2 --- /dev/null +++ b/test/expect_throws_message.h @@ -0,0 +1,143 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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. + */ + +// This code was taken from Drake. +// https://github.com/RobotLocomotion/drake/blob/master/common/test_utilities/expect_throws_message.h + +#ifndef FCL_EXPECT_THROWS_MESSAGE_H +#define FCL_EXPECT_THROWS_MESSAGE_H + +#include +#include + +#ifdef FCL_DOXYGEN_CXX + +/** Unit test helper macro for "expecting" an exception to be thrown but also +testing the error message against a provided regular expression. This is +like GTest's `EXPECT_THROW` but is fussier about the particular error message. +Usage example: @code + FCL_EXPECT_THROWS_MESSAGE( + StatementUnderTest(), // You expect this statement to throw ... + std::logic_error, // ... this exception with ... + ".*some important.*phrases.*that must appear.*"); // ... this message. +@endcode +The regular expression must match the entire error message. If there is +boilerplate you don't care to match at the beginning and end, surround with +`.*` to ignore. + +Following GTest's conventions, failure to perform as expected here is a +non-fatal test error. An `ASSERT` variant is provided to make it fatal. There +are also `*_IF_ARMED` variants. These require an exception in Debug builds. In +Release builds, the expression will pass if it _doesn't_ throw or if it throws +an exception that would pass the same test as in Debug builds. There is no +mechanism for testing _exclusive_ throwing behavior (i.e., only throws in +Debug). +@see FCL_ASSERT_THROWS_MESSAGE +@see FCL_EXPECT_THROWS_MESSAGE_IF_ARMED, FCL_ASSERT_THROWS_MESSAGE_IF_ARMED */ +#define FCL_EXPECT_THROWS_MESSAGE(expression, exception, regexp) + +/** Fatal error version of `FCL_EXPECT_THROWS_MESSAGE`. +@see FCL_EXPECT_THROWS_MESSAGE */ +#define FCL_ASSERT_THROWS_MESSAGE(expression, exception, regexp) + +/** Same as `FCL_EXPECT_THROWS_MESSAGE` in Debug builds, but doesn't require +a throw in Release builds. However, if the Release build does throw it must +throw the right message. More precisely, the thrown message is required +whenever `FCL_ENABLE_ASSERTS` is defined, which Debug builds do by default. +@see FCL_EXPECT_THROWS_MESSAGE */ +#define FCL_EXPECT_THROWS_MESSAGE_IF_ARMED(expression, exception, regexp) + +/** Same as `FCL_ASSERT_THROWS_MESSAGE` in Debug builds, but doesn't require +a throw in Release builds. However, if the Release build does throw it must +throw the right message. More precisely, the thrown message is required +whenever `FCL_ENABLE_ASSERTS` is defined, which Debug builds do by default. +@see FCL_ASSERT_THROWS_MESSAGE */ +#define FCL_ASSERT_THROWS_MESSAGE_IF_ARMED(expression, exception, regexp) + +#else // FCL_DOXYGEN_CXX + +#define FCL_EXPECT_THROWS_MESSAGE_HELPER(expression, exception, regexp, \ + must_throw, fatal_failure) \ +try { \ + expression; \ + if (must_throw) { \ + if (fatal_failure) { \ + GTEST_FATAL_FAILURE_("\t" #expression " failed to throw " #exception); \ + } else { \ + GTEST_NONFATAL_FAILURE_("\t" #expression " failed to throw " #exception);\ + } \ + } \ +} catch (const exception& err) { \ + auto matcher = [](const char* s, const std::string& re) { \ + return std::regex_match(s, std::regex(re)); }; \ + if (fatal_failure) { \ + ASSERT_PRED2(matcher, err.what(), regexp); \ + } else { \ + EXPECT_PRED2(matcher, err.what(), regexp); \ + } \ +} + +#define FCL_EXPECT_THROWS_MESSAGE(expression, exception, regexp) \ + FCL_EXPECT_THROWS_MESSAGE_HELPER(expression, exception, regexp, \ + true /*must_throw*/, false /*non-fatal*/) + +#define FCL_ASSERT_THROWS_MESSAGE(expression, exception, regexp) \ + FCL_EXPECT_THROWS_MESSAGE_HELPER(expression, exception, regexp, \ + true /*must_throw*/, true /*fatal*/) + +#ifdef NDEBUG +// Throwing the expected message is optional in this case. + +#define FCL_EXPECT_THROWS_MESSAGE_IF_DEBUG(expression, exception, regexp) \ + FCL_EXPECT_THROWS_MESSAGE_HELPER(expression, exception, regexp, \ + false /*optional*/, false /*non-fatal*/) + +#define FCL_ASSERT_THROWS_MESSAGE_IF_DEBUG(expression, exception, regexp) \ + FCL_EXPECT_THROWS_MESSAGE_HELPER(expression, exception, regexp, \ + false /*optional*/, true /*fatal*/) + +#else // NDEBUG +// Throwing the expected message is required in this case. + +#define FCL_EXPECT_THROWS_MESSAGE_IF_DEBUG(expression, exception, regexp) \ + FCL_EXPECT_THROWS_MESSAGE(expression, exception, regexp) + +#define FCL_ASSERT_THROWS_MESSAGE_IF_DEBUG(expression, exception, regexp) \ + FCL_ASSERT_THROWS_MESSAGE(expression, exception, regexp) + +#endif // NDEBUG + +#endif // FCL_DOXYGEN_CXX + +#endif // FCL_EXPECT_THROWS_MESSAGE_H diff --git a/test/geometry/CMakeLists.txt b/test/geometry/CMakeLists.txt new file mode 100644 index 000000000..a87b0abfb --- /dev/null +++ b/test/geometry/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(shape) diff --git a/test/geometry/shape/CMakeLists.txt b/test/geometry/shape/CMakeLists.txt new file mode 100644 index 000000000..b227271ab --- /dev/null +++ b/test/geometry/shape/CMakeLists.txt @@ -0,0 +1,8 @@ +set(tests + test_convex.cpp + ) + +# Build all the tests +foreach(test ${tests}) + add_fcl_test(${test}) +endforeach(test) diff --git a/test/geometry/shape/test_convex.cpp b/test/geometry/shape/test_convex.cpp new file mode 100644 index 000000000..c4d9ea549 --- /dev/null +++ b/test/geometry/shape/test_convex.cpp @@ -0,0 +1,532 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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 CNRS-LAAS and AIST 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) (2018) */ + +// Tests the implementation of a convex polytope geometry. + +#include "fcl/geometry/shape/convex.h" + +#include + +#include +#include + +#include "eigen_matrix_compare.h" +#include "fcl/common/types.h" + +namespace fcl { +namespace { + +using std::max; + +// Necessary to satisfy Eigen's alignment requirements. See +// http://eigen.tuxfamily.org/dox-devel/group__TopicStlContainers.html#StlContainers_vector +template +using PoseVector = std::vector, + Eigen::aligned_allocator>>; + +// Utilities to print scalar type in error messages. +template +struct ScalarString { + static std::string value() { return "unknown"; } +}; + +template <> +struct ScalarString { + static std::string value() { return "double"; } +}; + +template <> +struct ScalarString { + static std::string value() { return "float"; } +}; + +// Base definition of a "unit" convex polytope. Specific instances should define +// faces, vertices, and quantities such as volume, center of mass, and moment of +// inertia in terms of a scale factor. +template +class Polytope { + public: + explicit Polytope(S scale) + : vertices_(std::make_shared>>()), + polygons_(std::make_shared>()), scale_(scale) {} + + Polytope(const Polytope &other) + : vertices_(std::make_shared>>(*(other.vertices_))), + polygons_(std::make_shared>(*(other.polygons_))), + scale_(other.scale_) {} + + virtual int face_count() const = 0; + virtual int vertex_count() const = 0; + virtual S volume() const = 0; + virtual Vector3 com() const = 0; + virtual Matrix3 principal_inertia_tensor() const = 0; + virtual std::string description() const = 0; + + // The scale of the polytope to use with test tolerances. + S scale() const { return scale_; } + std::shared_ptr>> points() const { + return vertices_; + } + std::shared_ptr> polygons() const { + return polygons_; + } + Convex MakeConvex() const { + // The Polytope class makes the pointers to vertices and faces const access. + // The Convex class calls for non-const pointers. Temporarily const-casting + // them to make it compatible. + return Convex(points(), face_count(), polygons()); + } + Vector3 min_point() const { + Vector3 m; + m.setConstant(std::numeric_limits::max()); + for (const auto& v : *vertices_) { + for (int i = 0; i < 3; ++i) { + if (v(i) < m(i)) m(i) = v(i); + } + } + return m; + } + Vector3 max_point() const { + Vector3 m; + m.setConstant(-std::numeric_limits::max()); + for (const auto& v : *vertices_) { + for (int i = 0; i < 3; ++i) { + if (v(i) > m(i)) m(i) = v(i); + } + } + return m; + } + Vector3 aabb_center() const { + return (max_point() + min_point()) / 2; + } + S aabb_radius() const { return (min_point() - aabb_center()).norm(); } + void SetPose(const Transform3& X_WP) { + for (auto& v : *vertices_) { + v = X_WP * v; + } + } + + protected: + void add_vertex(const Vector3& vertex) { vertices_->push_back(vertex); } + void add_face(std::initializer_list indices) { + polygons_->push_back(static_cast(indices.size())); + polygons_->insert(polygons_->end(), indices); + } + // Confirms the number of vertices and number of polygons matches the counts + // implied by vertex_count() and face_count(), respectively. + void confirm_data() { + // Confirm point count. + GTEST_ASSERT_EQ(vertex_count(), static_cast(vertices_->size())); + + // Confirm face count. + // Count the number of faces encoded in polygons_; + int count = 0; + int i = 0; + while (i < static_cast(polygons_->size())) { + ++count; + i += (*polygons_)[i] + 1; + } + GTEST_ASSERT_EQ(i, static_cast(polygons_->size())) + << "Badly defined polygons"; + GTEST_ASSERT_EQ(face_count(), count); + } + + private: + std::shared_ptr>> vertices_; + std::shared_ptr> polygons_; + S scale_{}; +}; + +// A simple regular tetrahedron with edges of length `scale` centered on the +// origin. +template +class EquilateralTetrahedron : public Polytope { + public: + // Constructs the tetrahedron (of edge length `s`). + explicit EquilateralTetrahedron(S scale) : Polytope(scale), scale_(scale) { + // Tetrahedron vertices in the tet's canonical frame T. The tet is + // "centered" on the origin so that it's center of mass is simple [0, 0, 0]. + const S z_base = -1 / S(2 * sqrt(6.)); + Vector3 points_T[] = {{S(0.5), S(-0.5 / sqrt(3.)), z_base}, + {S(-0.5), S(-0.5 / sqrt(3.)), z_base}, + {S(0), S(1. / sqrt(3.)), z_base}, + {S(0), S(0), S(sqrt(3. / 8))}}; + for (const auto& v : points_T) { + this->add_vertex(scale * v); + }; + + // Now add the polygons + this->add_face({0, 1, 2}); + this->add_face({1, 0, 3}); + this->add_face({0, 2, 3}); + this->add_face({2, 1, 3}); + + this->confirm_data(); + } + // Properties of the polytope. + int face_count() const final { return 4; } + int vertex_count() const final { return 4; } + virtual S volume() const final { + // This assumes unit mass. + S s = this->scale(); + return S(sqrt(2) / 12) * s * s * s; + } + virtual Vector3 com() const final { return Vector3::Zero(); } + virtual Matrix3 principal_inertia_tensor() const { + // TODO(SeanCurtis-TRI): Replace this with a legitimate tensor. + throw std::logic_error("Not implemented yet"); + }; + std::string description() const final { + return "Tetrahedron with scale: " + std::to_string(this->scale()); + } + + private: + S scale_{0}; +}; + +// A simple cube with sides of length `scale`. +template +class Cube : public Polytope { + public: + Cube(S scale) : Polytope(scale) { + // Cube vertices in the cube's canonical frame C. + Vector3 points_C[] = {{S(-0.5), S(-0.5), S(-0.5)}, // v0 + {S(0.5), S(-0.5), S(-0.5)}, // v1 + {S(-0.5), S(0.5), S(-0.5)}, // v2 + {S(0.5), S(0.5), S(-0.5)}, // v3 + {S(-0.5), S(-0.5), S(0.5)}, // v4 + {S(0.5), S(-0.5), S(0.5)}, // v5 + {S(-0.5), S(0.5), S(0.5)}, // v6 + {S(0.5), S(0.5), S(0.5)}}; // v7 + for (const auto& v : points_C) { + this->add_vertex(scale * v); + } + + // Now add the polygons + this->add_face({1, 3, 7, 5}); // +x + this->add_face({0, 4, 6, 2}); // -x + this->add_face({4, 5, 7, 6}); // +y + this->add_face({0, 2, 3, 1}); // -y + this->add_face({6, 7, 3, 2}); // +z + this->add_face({0, 1, 5, 4}); // -z + + this->confirm_data(); + } + + // Polytope properties + int face_count() const final { return 6; } + int vertex_count() const final { return 8; } + virtual S volume() const final { + S s = this->scale(); + return s * s * s; + } + virtual Vector3 com() const final { return Vector3::Zero(); } + virtual Matrix3 principal_inertia_tensor() const { + S scale_sqd = this->scale() * this->scale(); + // This assumes unit mass. + return Eigen::DiagonalMatrix(1. / 6., 1. / 6., 1. / 6.) * scale_sqd; + }; + std::string description() const final { + return "Cube with scale: " + std::to_string(this->scale()); + } +}; + +void testConvexConstruction() { + Cube cube{1}; + // Set the cube at some other location to make sure that the interior point + // test/ doesn't pass just because it initialized to zero. + Vector3 p_WB(1, 2, 3); + cube.SetPose(Transform3(Eigen::Translation3d(p_WB))); + Convex convex = cube.MakeConvex(); + + // This doesn't depend on the correct logic in the constructor. But this is + // as convenient a time as any to test that it reports the right node type. + EXPECT_EQ(convex.getNodeType(), GEOM_CONVEX); + + // The constructor computes the interior point. + EXPECT_TRUE(CompareMatrices(convex.getInteriorPoint(), p_WB)); +} + +template